# Eckstein, Bertsekas, "On the Douglas-Rachford Splitting Method and the
# Proximal Point Algorithm for Maximal Monotone Operators",
# Mathematical Programming, vol. 55, no. 1, pp. 293-318 (1989).

using Base.Iterators
using ProximalAlgorithms.IterationTools
using ProximalOperators: Zero
using LinearAlgebra
using Printf

struct DRS_iterable{R <: Real, C <: Union{R, Complex{R}}, T <: AbstractArray{C}, Tf, Tg}
    f::Tf
    g::Tg
    x::T
    gamma::R
end

Base.IteratorSize(::Type{<:DRS_iterable}) = Base.IsInfinite()

mutable struct DRS_state{T}
    x::T
    y::T
    r::T
    z::T
    res::T
end

DRS_state(iter::DRS_iterable) = DRS_state(copy(iter.x), zero(iter.x), zero(iter.x), zero(iter.x), zero(iter.x))

function Base.iterate(iter::DRS_iterable, state::DRS_state=DRS_state(iter))
    prox!(state.y, iter.f, state.x, iter.gamma)
    state.r .= 2 .*state.y .- state.x
    prox!(state.z, iter.g, state.r, iter.gamma)
    state.res .= state.y .- state.z
    state.x .-= state.res
    return state, state
end

# Solver

struct DouglasRachford{R}
    gamma::R
    maxit::Int
    tol::R
    verbose::Bool
    freq::Int

    function DouglasRachford{R}(; gamma::R, maxit::Int=1000, tol::R=R(1e-8),
        verbose::Bool=false, freq::Int=100
    ) where R
        @assert gamma > 0
        @assert maxit > 0
        @assert tol > 0
        @assert freq > 0
        new(gamma, maxit, tol, verbose, freq)
    end
end

function (solver::DouglasRachford{R})(
    x0::AbstractArray{C}; f=Zero(), g=Zero()
) where {R, C <: Union{R, Complex{R}}}

    stop(state::DRS_state) = norm(state.res, Inf) / solver.gamma <= solver.tol
    disp((it, state)) = @printf("%5d | %.3e\n", it, norm(state.res, Inf) / solver.gamma)

    iter = DRS_iterable(f, g, x0, solver.gamma)
    iter = take(halt(iter, stop), solver.maxit)
    iter = enumerate(iter)
    if solver.verbose iter = tee(sample(iter, solver.freq), disp) end

    num_iters, state_final = loop(iter)

    return state_final.y, state_final.z, num_iters

end

# Outer constructors

"""
    DouglasRachford([gamma, maxit, tol, verbose, freq])

Instantiate the Douglas-Rachford splitting algorithm (see [1]) for solving
convex optimization problems of the form

    minimize f(x) + g(x),

If `solver = DouglasRachford(args...)`, then the above problem is solved with

    solver(x0, [f, g])

Optional keyword arguments:

* `gamma::Real` (default: `1.0`), stepsize parameter.
* `maxit::Integer` (default: `1000`), maximum number of iterations to perform.
* `tol::Real` (default: `1e-8`), absolute tolerance on the fixed-point residual.
* `verbose::Bool` (default: `true`), whether or not to print information during the iterations.
* `freq::Integer` (default: `100`), frequency of verbosity.

References:

[1] Eckstein, Bertsekas, "On the Douglas-Rachford Splitting Method and the
Proximal Point Algorithm for Maximal Monotone Operators",
Mathematical Programming, vol. 55, no. 1, pp. 293-318 (1989).
"""
DouglasRachford(::Type{R}; kwargs...) where R = DouglasRachford{R}(; kwargs...)
DouglasRachford(; kwargs...) = DouglasRachford(Float64; kwargs...)


using SparseArrays, Graphs

A = sprand(Bool, 10, 20, 0.5)
for i in 1:10; A[i,i] = 0; end
dropzeros!(A)
g = FactorGraph(A)

@testset "factor graph" begin
    @testset "basics" begin
        @test is_bipartite(g)
        @test !is_directed(g)
        @test length(collect(edges(g))) == ne(g)
        @test length(collect(vertices(g))) == nv(g)
    end

    @testset "neighbors of variable" begin
        i = 3
        es = edges(g, Variable(i))
        neigs = neighbors(g, Variable(i))
        @test all( (e.a, e.i) == (a, i) for (e, a) in zip(es, neigs) )
    end

    @testset "neighbors of factor" begin
        a = 9
        es = edges(g, Factor(a))
        neigs = neighbors(g, Factor(a))
        @test all( (e.a, e.i) == (a, i) for (e, i) in zip(es, neigs) )
    end

    @testset "edge indexing" begin
        @test all( e == get_edge(g, Factor(e.a), Variable(e.i)) for e in edges(g) )
        @test all( e == get_edge(g, idx(e)) for e in edges(g) )
    end

    @testset "bipartiteness" begin
        gg = bipartite_view(g)
        @test is_bipartite(gg)
    end
end


__precompile__()

####################
module SDELab2
import NLsolve
import ForwardDiff

include("iteratedintegral.jl")
import .iteratedintegral

export set_opt, sde_strong_solution, set_fcn,  method_code, do_plotting

method_code=Dict()
method_code["SIE0"]="Strong Ito Euler "
method_code["SSEH0"]="Strong Stratonovich Euler-Heun "
method_code["SIE1"]="Strong Ito Euler (drift implicit)"
method_code["SSEH1"]="Strong Stratonovich Euler (drift implicit)"
method_code["SIE"]="Strong Ito Euler (alpha drift-implicit)"
method_code["SSEH"]="Strong Stratonovich Euler (alpha drift-implicit)"
method_code["SIM0"]="Strong Ito Milstein explicit"
method_code["SSM0"]="Strong Stratonovich Milstein explicit"
method_code["SIM1"]="Strong Ito Milstein (drift implicit)"
method_code["SSM1"]="Strong Stratonovich Milstein (drift implicit)"
method_code["SIM"]="Strong Ito Milstein (alpha drift-implicit)"
method_code["SSM"]="Strong Stratonovich Milstein (alpha drift-implicit)"
method_code["SIBDF"]="Strong Ito BDF-2"
# sdelab_gq
method_code["MFEM0"]="Mean-field SDE Gauss-quadrature Euler-Maruyama"
method_code["MFEM2"]="Mean-field SDE Gauss-quadrature second-order explicit"
method_code["MFEM"] ="Mean-field SDE Gauss-quadrature alpha-drift implicit (first order)"
method_code["MFEM0e"]="Mean-field SDE Gauss-quadrature Euler-Maruyama (exterior mean-field evaluation)"
method_code["MFEMe"] ="Mean-field SDE Gauss-quadrature alpha-drift implicit (first order, exterior mean-field)"

include("gauss_quadrature.jl")
using .gauss_quadrature
include("sdelab_gq.jl")
using .sdelab_gq
#####
function __init__()
end
##########################################
function get_increment!(x)
  x.dW[:]=sqrt(x.dt)*randn(x.m)
end
##########################################
# explicit Euler methods (i.e., Euler-Maruyama and Heun)
# data structure to store required bits
# and one-step routines
struct EM
  m
  drift
  diff_mat
  dt
  dW
end
# Ito version
function one_step_em(u,x::EM)
  get_increment!(x)
  un=u+x.drift(u)*x.dt+x.diff_mat(u,x.dW)
  return un
end
# Stratonovich (Heun method)
function one_step_heun(u,x::EM)
  get_increment!(x)
  uaux=u+x.diff_mat(u,x.dW)
  un=u+x.drift(u)*x.dt+0.5*(x.diff_mat(u,x.dW)+x.diff_mat(uaux,x.dW))
  return un
end
##################################################
# implicit Euler methods
# parameters for nonlinear solver
struct NLSOLVE_param
  maxfeval
  ftol
  xtol
end
# required bits for implicit-type Euler methods
struct EMImp
  m
  drift
  diff_mat
  Alpha
  dt
  dW
  nonlin
  jnonlin
  nlsolve::NLSOLVE_param
end
# Ito version of onestep
function one_step_em_alpha(u,x::EMImp)
  get_increment!(x)
  up=u + (1-x.Alpha)*x.drift(u)*x.dt + x.diff_mat(u,x.dW)
  solve=NLsolve.nlsolve((u,out)->x.nonlin(u,up,out),x.jnonlin,u, iterations=x.nlsolve.maxfeval, ftol=x.nlsolve.ftol, xtol=x.nlsolve.xtol)
  return solve.zero
end
# Stratonovich version of onestep
function one_step_heun_alpha(u,x::EMImp)
  get_increment!(x)
  uaux=u+x.diff_mat(u,x.dW)
  up=u+ (1-x.Alpha)*x.drift(u)*x.dt+0.5*(x.diff_mat(u,x.dW)+x.diff_mat(uaux,x.dW))
  solve=NLsolve.nlsolve((u,out)->x.nonlin(u,up,out),x.jnonlin,u)
  return solve.zero
end
# BDF method
# required bits
struct BDF
  m
  drift
  diff_mat
  dt
  dW
  dWp
  up
  nonlin
  jnonlin
  nlsolve::NLSOLVE_param
end
# Ito BDF
function one_step_bdf(u,x::BDF)
  get_increment!(x)
  up=(4/3)*u -(1/3)*x.up +x.diff_mat(u,x.dW)-(1/3)* x.diff_mat(x.up,x.dWp)
  solve=NLsolve.nlsolve((u,out)->x.nonlin(u,up,out),x.jnonlin,u, iterations=x.nlsolve.maxfeval, ftol=x.nlsolve.ftol, xtol=x.nlsolve.xtol)
  x.up[:]=u
  x.dWp[:]=x.dW
  return solve.zero
end
##########################################################
# Explicit Milstein methods
struct Mil
  d
  m
  drift
  diff_mat
  Jdiff_vecs
  S
  dt
end
# Milstein explicit, Stratonovich version
function one_step_strat_mil(u,x::Mil)
  IteratedIntegral.get_strat!(x.dt,x.S)
  J=x.S["J"];  K=x.S["K"]
  un=u+x.drift(u)*x.dt+x.diff_mat(u,J)
  for i=1:x.m
    Ki=reshape(K[i,:],(x.m,1))
    un+=x.Jdiff_vecs[i](u)*x.diff_mat(u,Ki)
  end
  return reshape(un,x.d)
end
# Milstein explicit, Ito version
function one_step_ito_mil(u,x::Mil)
  IteratedIntegral.get_ito!(x.dt,x.S)
  J=x.S["J"];  K=x.S["K"]
  un=u+x.drift(u)*x.dt+x.diff_mat(u,J)
  for i=1:x.m
    Ki=reshape(K[i,:],(x.m,1))
    un+=x.Jdiff_vecs[i](u)*x.diff_mat(u,Ki)
  end
  return reshape(un,x.d )
end
###################################################
# Implicit Milstein methods
struct MilImp
  d
  m
  drift
  diff_mat
  Jdiff_vecs
  Alpha
  S
  dt
  nonlin
  jnonlin
  nlsolve::NLSOLVE_param
end
# Milstein implicit Stratonovich
function one_step_strat_mil_alpha(u,x::MilImp)
  IteratedIntegral.get_strat!(x.dt,x.S)
  J=x.S["J"];   K=x.S["K"]
  up=u + x.Alpha*x.drift(u)*x.dt + x.diff_mat(u,J)
  for i=1:x.m
    Ki=reshape(K[i,:],(x.m,1))
    up+=x.Jdiff_vecs[i](u)*x.diff_mat(u,Ki)
  end
  solve=NLsolve.nlsolve((u,out)->x.nonlin(u,up,out),x.jnonlin,u,iterations=x.nlsolve.maxfeval, ftol=x.nlsolve.ftol, xtol=x.nlsolve.xtol)
  return solve.zero
end
# Milstein implicit Ito
function one_step_ito_mil_alpha(u,x::MilImp)
  IteratedIntegral.get_ito!(x.dt,x.S)
  J=x.S["J"];   K=x.S["K"]
  up=u+ x.Alpha*x.drift(u)*x.dt +x.diff_mat(u,J)
  for i=1:x.m
    Ki=reshape(K[i,:],(x.m,1))
    up += x.Jdiff_vecs[i](u)*x.diff_mat(u,Ki)
  end
  solve=NLsolve.nlsolve((u,out)->x.nonlin(u,up,out), x.jnonlin, u, iterations=x.nlsolve.maxfeval, ftol=x.nlsolve.ftol, xtol=x.nlsolve.xtol)
  return solve.zero
end
###################################################
# Set solver options (many defaults)
function set_opt(maxStepSize,solver)
  opt=Dict()
  opt["MaxStepSize"]=maxStepSize
  opt["Solver"]=solver
  opt["Alpha"]=0.5
  opt["nlsolve_param"]=NLSOLVE_param(10,1e-6,1e-8)
  opt["max_p"]=1000
  opt["calc_final_W"]=true
  opt["is_ito"]=(solver[1:2]=="SI")
  # plotting
  opt["always_clear"]=true
  opt["do_plotting"]=false
  opt["output_sel"]=[1,2]
  return opt
end
#
#
function sde_strong_solution(fcn, tspan, y0, opt)
  #=
  many driver routine:
  fcn= SDE definition
  tspan= vector of required times
  y0=value of sample path at first time in tspan
  opt=solver options
  =#
  ts=minimum(tspan); tf=maximum(tspan)
  N=round((tf-ts)/opt["MaxStepSize"]); dt=(tf-ts)/N
  solver=opt["Solver"]
  t_out=Array{Float64}(undef,length(tspan))
  y_out=Array{Float64}(undef,length(tspan),fcn.d)
  y=deepcopy(y0); y_out[1,:]=y # initial data
  t=ts; t_out[1]=ts # initial time
  #
  Wf=zeros(fcn.m)
  #
  if solver=="SIE0" || solver=="SSEH0"# StrongItoEuler Alpha=0 explicit
    x=EM(fcn.m,fcn.drift,fcn.diff_mat,dt,zeros(fcn.m))
    dW=x.dW
    if solver=="SIE0"
      call_onestep(y)=(one_step_em(y,x))
    else
      call_onestep(y)=(one_step_heun(y,x))
    end
  elseif solver=="SIE1" || solver=="SSEH1"# strongItoEuler Alpha=1 implicit
    nlsolve_param=opt["nlsolve_param"]
    function nonlin!(un_,up,out)
      out[:] =(un_-up-fcn.drift(un_)*dt) # must avoid recreating out!
    end
    function jnonlin!(un_,out)
      out[:,:]=eye(fcn.d)-fcn.Jdrift(un_)*dt
    end
    x=EMImp(fcn.m,fcn.drift,fcn.diff_mat,1.,dt,zeros(fcn.m),nonlin!,jnonlin!,nlsolve_param)
    dW=x.dW
    if solver=="SIE1"
      call_onestep(y)=(one_step_em_alpha(y,x))
    else
      call_onestep(y)=(one_step_heun_alpha(y,x))
    end
  elseif solver=="SIE" || solver =="SSEH"# strongItoEuler Alpha parameter
    Alpha=opt["Alpha"]
    nlsolve_param=opt["nlsolve_param"]
    function nonlin!(un_,up,out)
      out[:] =(un_-up-Alpha*fcn.drift(un_)*dt) # must avoid recreating out!
    end
    function jnonlin!(un_,out)
      out[:,:]=eye(fcn.d)-Alpha*fcn.Jdrift(un_)*dt
    end
    x=EMImp(fcn.m,fcn.drift,fcn.diff_mat,Alpha,dt,zeros(fcn.m),nonlin!,jnonlin!, nlsolve_param)
    dW=x.dW
    if solver=="SIE"
      call_onestep(y)=(one_step_em_alpha(y,x))
    else
      call_onestep(y)=(one_step_heun_alpha(y,x))
    end
  elseif solver=="SIM0" || solver=="SSM0"# StrongItoMilstein Alpha=0 explicit
    S=IteratedIntegral.init(fcn,opt)
    x=Mil(fcn.d,fcn.m,fcn.drift,fcn.diff_mat,fcn.Jdiff_vecs,S,dt)
    dW=x.S["J"]
    if solver=="SIM0"
      call_onestep(y)=(one_step_ito_mil(y,x))
    else
      call_onestep(y)=(one_step_strat_mil(y,x))
    end
  elseif solver=="SIM1" || solver=="SSM1"# StrongItoMilstein Alpha=1 implicit
      S=IteratedIntegral.init(fcn,opt)
      nlsolve_param=opt["nlsolve_param"]
      function nonlin!(un_,up,out)
        out[:] =(un_-up-fcn.drift(un_)*dt) # must avoid recreating out!
      end
      function jnonlin!(un_,out)
        out[:,:]=eye(fcn.d)-fcn.Jdrift(un_)*dt
      end
      x=MilImp(fcn.d,fcn.m,fcn.drift,fcn.diff_mat,fcn.Jdiff_vecs,0.,S,dt,nonlin!,jnonlin!,nlsolve_param)
      dW=x.S["J"]
      if solver=="SIM1"
        call_onestep(y)=(one_step_ito_mil_alpha(y,x))
      else
        call_onestep(y)=(one_step_strat_mil_alpha(y,x))
      end
    elseif solver=="SIM" || solver=="SSM"
      S=IteratedIntegral.init(fcn,opt)
      nlsolve_param=opt["nlsolve_param"]
      Alpha=opt["Alpha"]
      function nonlin!(un_,up,out)
        out[:] =(un_-up-Alpha*fcn.drift(un_)*dt) # must avoid recreating out!
      end
      function jnonlin!(un_,out)
        out[:,:]=eye(fcn.d)-Alpha*fcn.Jdrift(un_)*dt
      end
      x=MilImp(fcn.d,fcn.m,fcn.drift,fcn.diff_mat,fcn.Jdiff_vecs,Alpha,S,dt,nonlin!,jnonlin!,nlsolve_param)
      dW=x.S["J"]
      if solver=="SIM"
        call_onestep(y)=(one_step_ito_mil_alpha(y,x))
      else
        call_onestep(y)=(one_step_strat_mil_alpha(y,x))
      end
  elseif solver=="SIBDF"
    nlsolve_param=opt["nlsolve_param"]
    # starting method
    function nonlin0!(un_,up,out)
      out[:] =(un_-up-fcn.drift(un_)*dt) # must avoid recreating out!
    end
    function jnonlin0!(un_,out)
      out[:,:]=eye(fcn.d)-fcn.Jdrift(un_)*dt
    end
    x0=EMImp(fcn.m,fcn.drift,fcn.diff_mat,1.,dt,zeros(fcn.m),nonlin0!,jnonlin0!,nlsolve_param)
    y1=one_step_em_alpha(y,x0)
    t=t+dt
    Wf+=x0.dW
    # bdf method
    function nonlin!(un_,up,out)
      out[:] =(un_-up-(2/3)*fcn.drift(un_)*dt) # must avoid recreating out!
    end
    function jnonlin!(un_,out)
      out[:,:]=eye(fcn.d)-(2/3)*fcn.Jdrift(un_)*dt
    end
    x=BDF(fcn.m,fcn.drift,fcn.diff_mat,dt,zeros(fcn.m),x0.dW,deepcopy(y0),nonlin!,jnonlin!,nlsolve_param)
    y=y1;
    dW=x.dW
    call_onestep(y)=(one_step_bdf(y,x))
  end

  #
  for j=2:(length(tspan)) # loop over required times
    N=round(Int,(tspan[j]-t)/dt) # steps to next tspan node
    for i=1:N # does not run if N=0
      t=t+dt
      y=call_onestep(y)
      if opt["calc_final_W"]==true
        Wf+=dW
      end
    end
    t_out[j]=t
    y_out[j,:]=y # constant approx if tspan too fine relative to dt
  end
  try
    do_plotting(t_out,y_out,opt)
  catch
    print("plotting failed")
  end
  return t_out,y_out,Wf
end
#
using PyPlot
function do_plotting(t,y,opt)
  if opt["do_plotting"]
    if opt["always_clear"]
      clf()
    end
    str=opt["output_plot_type"]
    if str=="path_plot"
      sel=opt["output_sel"]
      plot(t,y[:,sel])
    elseif str=="phase_plot"
      sel=opt["output_sel"]
      plot(y[:,sel[1]], y[:,sel[2]])
    elseif str=="time_phase"
      print("not yet implemented :-(")
    elseif str=="phase3_plot"
      print("not yet implemented :-(")
    end
  end
end
#
struct Fcn
  d # state-space dimension
  m # no. Brownian motions
  drift # u -> f(u)
  Jdrift # Jacobian of drift (auto computed)
  diff_mat # (u,dw)-> g(u)*dw
  diff_vecs # [i]: u -> g_i(u) for i=1:m
  Jdiff_vecs # Jacobian of diffusion vectors (auto computed)
  noise_type # 1=unstructure; 2=diagonal; 3=commutative
end
#
function set_fcn(d,m,drift,diff_mat,diff_vecs,noise_type=1)
  #=
  Define sde via Fcn type
  =#
  Jdrift=ForwardDiff.jacobian(drift)
  if diff_vecs!=Union{}
    Jdiff_vecs=Array{Function}(undef,m)
    for i=1:m
      Jdiff_vecs[i]=ForwardDiff.jacobian(diff_vecs[i])
    end
  else
    Jdiff_vecs=Union{}
  end
  fcn=Fcn(d,m,drift,Jdrift,diff_mat,diff_vecs,Jdiff_vecs,noise_type)
end
#
end #sdelab2
#


using Images, TestImages, StaticArrays, ImageTracking, Random, OffsetArrays, CoordinateTransformations, BenchmarkTools

img1 = Gray{Float64}.(testimage("mandrill"))
trans = Translation(0, -3.0)
img2 = warp(img1, trans, axes(img1))

corners = imcorner(img1, method=shi_tomasi)
I = findall(!iszero, corners)
r, c = (getindex.(I, 1), getindex.(I, 2))
points = map((ri, ci) -> SVector{2}(Float64(ri), Float64(ci)), r, c)

flow, indicator  = optical_flow(img1, img2, points, LucasKanade( 11, 4, 20, 0.000001))

@btime flow, indicator  =  optical_flow($img1, $img2, $points, LucasKanade( 11, 4, 20, 0.000001))

(sum(indicator)/ length(indicator)) * 100

@code_warntype  optical_flow(img1, img2, points, LucasKanade( 11, 4, 20, 0.000001))


"""
    group_generator_basic(generators::Array{T}; prnt=false) -> Set(group), num_all_mult

Calculate all other elements of this closed group by multiplying all generators and storing new elements and then multiplying the generators again with the new elements.
Very efficient with ``O(\\Omega \\cdot N)`` multiplications.
It is the most efficient algorithm for a given set of generators. Only the use of smaller sets of generators can reduce the number of multiplications further.

The `prnt` variable can be used to specify if the programm should output the progressbar. This can slow things down.
"""
function group_generator_basic(generators::Array{T}; prnt = false)::Tuple{Set{T},Int64} where T

    # Sets to store generated elements in different levels of generation
    group_prev_level = Set([])
    group_current_level = Set(copy(generators))
    group_next_level = Set([])

    level = 0
    size_after = 0
    new_elements = 0

    num_all_mult = 0

    while length(group_current_level) > 0
        level += 1
        num_multiplications = 0

        # calculate the number of multiplications in this level
        num_max_mult = length(group_current_level) * (length(group_current_level) + length(group_prev_level))

        if prnt
            prog = Progress(num_max_mult)
        end

        # empty next for new iteration
        group_next_level = Set([])

        # multiplicate current elements with all generators and save new elements to next
        for M in generators
            for N in group_current_level
                num_multiplications += 1
                el = M * N

                #union!(group_next_level, [el]) # slower alternative
                push!(group_next_level, el)

                # space optimization was replaced by switching to the Set data-type.
                #if num_multiplications % 1_000_000 == 0
                    # free up some space every 1 000 000 iterations (decrease RAM usage from 20+ GB to 2-3 GB)
                    #unique!(group_next_level)
                #end

                if prnt && num_multiplications % 1_000 in [0,1]
                    size_next_level = length(group_next_level)
                    update!(prog, num_multiplications; showvalues = [(:i,"$num_multiplications/$num_max_mult"),(:level, level),(:size,size_after),(:new_elements, new_elements),(:size_next_level, "$size_next_level ($(size_next_level/num_multiplications * 100)%)")])
                end
            end
        end

        if prnt print("current → prev...") end

        # current is now done, move to previous
        union!(group_prev_level, group_current_level)

        if prnt println(" done.") end

        if prnt print("next → current...") end

        # move all from next to current that are not in previous
        group_current_level = setdiff(group_next_level, group_prev_level)

        if prnt println(" done.") end

        # some stats to display
        size_after = length(group_prev_level)
        new_elements = length(group_current_level)
        num_all_mult += num_multiplications
        println("level #",level," size ", size_after, " and ", new_elements," new elements (",num_multiplications," multiplications)")
        if prnt
            println("level #",level," size ", size_after, " and ", new_elements," new elements (",num_multiplications," multiplications)")
            # countdown to prevent directly overwriting with progressbar
            print("5..."); sleep(1); print("4..."); sleep(1); print("3..."); sleep(1); print("2..."); sleep(1); println("1..."); sleep(1);
        end
    end

    println("\nEnded after ", level, " iterations. \nThe resulting group has ", size_after, " elements.")

    # returns the final group and the number of multiplications needed
    return group_prev_level, num_all_mult
end


n = rand(1:38)
mat = matrixdepot(n)
matlist = matrixdepot(1:n)


function print_summary(result::TSPDroneResult)
    report = generate_summary(result)
    println(report)
end

fmt(n) = n #round(n, digits=5)

function generate_summary(result::TSPDroneResult)
    combined_nodes = intersect(result.truck_route, result.drone_route)
    operations = [(combined_nodes[i], combined_nodes[i+1]) for i in 1:length(combined_nodes)-1]

    op_costs = Float64[]
    io = IOBuffer()

    for i in 1:length(operations)
        op = operations[i]
        truck_length, truck_sub_route =
            operation_length(op[1], op[2], result.truck_route, result.Ct)
        drone_length, drone_sub_route =
            operation_length(op[1], op[2], result.drone_route, result.Cd)

        op_length = max(truck_length, drone_length)
        push!(op_costs, op_length)

        println(io, "Operation #$(i):")
        println(io, "  - Truck        = $(fmt(truck_length)) : $(truck_sub_route)")
        println(io, "  - Drone        = $(fmt(drone_length)) : $(drone_sub_route)")
        println(io, "  - Length       = $(fmt(op_length))")
        if drone_length > result.flying_range
            write(io, "  - flying_range : ")
            printstyled(io, "FAIL\n", color=:red)
        end

    end

    @assert sum(op_costs) == result.total_cost

    write(io, "Total Cost = $(fmt(sum(op_costs)))")

    report = String(take!(io))

    return report
end

function operation_length(origin, destination, route, cost_mtx)
    o_idx = findfirst(x -> x == origin, route)
    d_idx = findfirst(x -> x == destination, route)
    sub_route = route[o_idx:d_idx]

    length = 0.0
    for i in o_idx:d_idx-1
        length += cost_mtx[route[i], route[i+1]]
    end

    return length, sub_route
end


# This example shows how to use RAFF.jl to detect circles drawn from
# the user. This example was inspired in `GtkReactive.jl` drawing
# example.

# Packages needed
#
# add Gtk
# add GtkReactive
# add Graphics
# add Colors

using Gtk, Gtk.ShortNames, GtkReactive, Graphics, Colors
using RAFF

win = Window("Drawing") |> (bx = Box(:v))

set_gtk_property!(bx, :spacing, 10)

cb = GtkReactive.dropdown(["RAFF", "Least Squares"])
push!(bx, cb)

c = canvas(UserUnit, 200, 200)       # create a canvas with user-specified coordinates
push!(bx, c)

const moddraw = Signal([]) # the model points
const newline = Signal([]) # the in-progress line (will be added to list above)

const drawing = Signal(false)  # this will become true if we're actively dragging

choice = map(x->(push!(moddraw, []);value(x)), cb)

function run_raff()

    n, model, = RAFF.model_list["circle"]

    np, = size(value(newline))

    data = Array{Float64, 2}(undef, np, 3)

    l = value(newline)

    for i in 1:np

        data[i,1] = l[i].x
        data[i,2] = l[i].y
        data[i,3] = 0.0

    end

    r = if value(choice) == "RAFF"

        raff(model, data, n, ftrusted=0.7, MAXMS=10)

    else

        raff(model, data, n, ftrusted=1.0, MAXMS=10)

    end
    println(r)
    # Build points for drawing the circle
    p = (α) -> [abs(r.solution[3]) * cos(α) + r.solution[1], abs(r.solution[3]) * sin(α) + r.solution[2]]

    push!(moddraw, [p(i) for i in LinRange(0.0, 2 * π, 100)])

end

# If has changed the selection box, then run RAFF again.
sigc = map(choice) do c

    run_raff()

end

sigstart = map(c.mouse.buttonpress) do btn
    # This is the beginning of the function body, operating on the argument `btn`
    if btn.button == 1 && btn.modifiers == 0 # is it the left button, and no shift/ctrl/alt keys pressed?
        push!(drawing, true)   # activate dragging
        push!(newline, [btn.position])  # initialize the line with the current position
        push!(moddraw, [])
    end
end

const dummybutton = MouseButton{UserUnit}()
# See the Reactive.jl documentation for `filterwhen`
sigextend = map(filterwhen(drawing, dummybutton, c.mouse.motion)) do btn
    # while dragging, extend `newline` with the most recent point
    push!(newline, push!(value(newline), btn.position))
end

sigend = map(c.mouse.buttonrelease) do btn
    if btn.button == 1
        push!(drawing, false)  # deactivate dragging
        run_raff()
    end
end

function drawline(ctx, l, color)
    isempty(l) && return
    p = first(l)
    move_to(ctx, p.x, p.y)
    set_source(ctx, color)
    for i = 2:length(l)
        p = l[i]
        line_to(ctx, p.x, p.y)
    end
    stroke(ctx)
end

function drawcircle(ctx, l, color)
    isempty(l) && return
    p = first(l)
    move_to(ctx, p[1], p[2])
    set_source(ctx, color)
    for i = 2:length(l)
        p = l[i]
        line_to(ctx, p[1], p[2])
    end
    stroke(ctx)
end

# Because `draw` isn't a one-line function, we again use do-block syntax:
redraw = draw(c, moddraw, newline) do cnvs, circ, newl  # the function body takes 3 arguments
    fill!(cnvs, colorant"white")   # set the background to white
    set_coordinates(cnvs, BoundingBox(0, 1, 0, 1))  # set coordinates to 0..1 along each axis
    ctx = getgc(cnvs)   # gets the "graphics context" object (see Cairo/Gtk)
    drawcircle(ctx, circ, colorant"blue")  # draw old lines in blue
    drawline(ctx, newl, colorant"red")    # draw new line in red
end

showall(win)

#If we are not in a REPL
if (!isinteractive())

    # Create a condition object
    c = Condition()

    # Get the window
    # win = guidict["gui"]["window"]

    # Notify the condition object when the window closes
    signal_connect(win, :destroy) do widget
        notify(c)
    end

    # Wait for the notification before proceeding ...
    wait(c)
end


# MLJ interface for Decision Trees/Random Forests models

import MLJModelInterface       # It seems that having done this in the top module is not enought
const MMI = MLJModelInterface  # We need to repoeat it here

export PerceptronClassifier, KernelPerceptronClassifier, PegasosClassifier

# ------------------------------------------------------------------------------
# Model Structure declarations..

mutable struct PerceptronClassifier <: MMI.Probabilistic
   initialθ::Vector{Float64}
   initialθ₀::Float64
   maxEpochs::Int64
   shuffle::Bool
   forceOrigin::Bool
   returnMeanHyperplane::Bool
   rng::AbstractRNG
end
PerceptronClassifier(;
  initialθ=Float64[],
  initialθ₀=0.0,
  maxEpochs=1000,
  shuffle=false,
  forceOrigin=false,
  returnMeanHyperplane=false,
  rng = Random.GLOBAL_RNG,
  ) = PerceptronClassifier(initialθ,initialθ₀,maxEpochs,shuffle,forceOrigin,returnMeanHyperplane,rng)

#kernelPerceptron(x, y; K=radialKernel, T=100, α=zeros(Int64,length(y)), nMsgs=0, shuffle=false)
 mutable struct KernelPerceptronClassifier <: MMI.Probabilistic
     K::Function
     maxEpochs::Int64
     initialα::Vector{Int64}
     shuffle::Bool
     rng::AbstractRNG
 end
 KernelPerceptronClassifier(;
    K=radialKernel,
    maxEpochs=100,
    initialα = Int64[],
    shuffle=false,
    rng = Random.GLOBAL_RNG,
    ) = KernelPerceptronClassifier(K,maxEpochs,initialα,shuffle,rng)

# pegasos(x, y; θ=zeros(size(x,2)),θ₀=0.0, λ=0.5,η= (t -> 1/sqrt(t)), T=1000, nMsgs=10, shuffle=false,forceOrigin=false,returnMeanHyperplane=false
mutable struct PegasosClassifier <: MMI.Probabilistic
   initialθ::Vector{Float64}
   initialθ₀::Float64
   λ::Float64
   η::Function
   maxEpochs::Int64
   shuffle::Bool
   forceOrigin::Bool
   returnMeanHyperplane::Bool
   rng::AbstractRNG
end
PegasosClassifier(;
  initialθ=Float64[],
  initialθ₀=0.0,
  λ = 0.5,
  η = (t -> 1/sqrt(t)),
  maxEpochs=1000,
  shuffle=false,
  forceOrigin=false,
  returnMeanHyperplane=false,
  rng = Random.GLOBAL_RNG,
  ) = PegasosClassifier(initialθ,initialθ₀,λ,η,maxEpochs,shuffle,forceOrigin,returnMeanHyperplane,rng)

# ------------------------------------------------------------------------------
# Fit functions...

function MMI.fit(model::PerceptronClassifier, verbosity, X, y)
 x = MMI.matrix(X)                     # convert table to matrix
 allClasses = levels(y)
 initialθ  = length(model.initialθ) == 0 ? zeros(size(x,2)) : model.initialθ
 fitresult = perceptron(x, y; θ=initialθ, θ₀=model.initialθ₀, T=model.maxEpochs, nMsgs=0, shuffle=model.shuffle, forceOrigin=model.forceOrigin, returnMeanHyperplane=model.returnMeanHyperplane,rng=model.rng)
 cache=nothing
 report=nothing
 return (fitresult,allClasses), cache, report
end

function MMI.fit(model::KernelPerceptronClassifier, verbosity, X, y)
 x          = MMI.matrix(X)                     # convert table to matrix
 allClasses = levels(y)
 initialα   = length(model.initialα) == 0 ? zeros(Int64,length(y)) : model.initialα
 fitresult  = kernelPerceptron(x, y; K=model.K, T=model.maxEpochs, α=initialα, nMsgs=0, shuffle=model.shuffle,rng=model.rng)
 cache      = nothing
 report     = nothing
 return (fitresult,allClasses), cache, report
end

function MMI.fit(model::PegasosClassifier, verbosity, X, y)
 x = MMI.matrix(X)                     # convert table to matrix
 allClasses = levels(y)
 initialθ  = length(model.initialθ) == 0 ? zeros(size(x,2)) : model.initialθ
 fitresult = pegasos(x, y; θ=initialθ,θ₀=model.initialθ₀, λ=model.λ,η=model.η, T=model.maxEpochs, nMsgs=0, shuffle=model.shuffle, forceOrigin=model.forceOrigin, returnMeanHyperplane=model.returnMeanHyperplane,rng=model.rng)
 cache=nothing
 report=nothing
 return (fitresult,allClasses), cache, report
end

# ------------------------------------------------------------------------------
# Predict functions....

function MMI.predict(model::Union{PerceptronClassifier,PegasosClassifier}, fitresult, Xnew)
    fittedModel      = fitresult[1]
    #classes          = CategoricalVector(fittedModel.classes)
    classes          = fittedModel.classes
    allClasses       = fitresult[2] # as classes do not includes classes unsees at training time
    nLevels          = length(allClasses)
    nRecords         = MMI.nrows(Xnew)
    modelPredictions = Perceptron.predict(MMI.matrix(Xnew), fittedModel.θ, fittedModel.θ₀, fittedModel.classes)
    predMatrix       = zeros(Float64,(nRecords,nLevels))
    # Transform the predictions from a vector of dictionaries to a matrix
    # where the rows are the PMF of each record
    for n in 1:nRecords
        for (c,cl) in enumerate(allClasses)
            predMatrix[n,c] = get(modelPredictions[n],cl,0.0)
        end
    end
    #predictions = [MMI.UnivariateFinite(classes, predMatrix[i,:])
    #               for i in 1:nRecords]
    predictions = MMI.UnivariateFinite(allClasses,predMatrix,pool=missing)
    return predictions
end

function MMI.predict(model::KernelPerceptronClassifier, fitresult, Xnew)
    fittedModel      = fitresult[1]
    #classes          = CategoricalVector(fittedModel.classes)
    classes          = fittedModel.classes
    allClasses       = fitresult[2] # as classes do not includes classes unsees at training time
    nLevels          = length(allClasses)
    nRecords         = MMI.nrows(Xnew)
    #ŷtrain = Perceptron.predict([10 10; 2.2 2.5],model.x,model.y,model.α, model.classes,K=model.K)
    modelPredictions = Perceptron.predict(MMI.matrix(Xnew), fittedModel.x, fittedModel.y, fittedModel.α, fittedModel.classes, K=fittedModel.K)
    predMatrix       = zeros(Float64,(nRecords,nLevels))
    # Transform the predictions from a vector of dictionaries to a matrix
    # where the rows are the PMF of each record
    for n in 1:nRecords
        for (c,cl) in enumerate(allClasses)
            predMatrix[n,c] = get(modelPredictions[n],cl,0.0)
        end
    end
    #predictions = [MMI.UnivariateFinite(classes, predMatrix[i,:])
    #              for i in 1:nRecords]
    #predictions = MMI.UnivariateFinite(classes, predMatrix)
    predictions = MMI.UnivariateFinite(allClasses,predMatrix,pool=missing)
    #predictions4 = MMI.UnivariateFinite(modelPredictions,pool=classes,ordered=false)
    #predictions = MMI.UnivariateFinite(modelPredictions,pool=fittedModel.classes)
    return predictions
end

# ------------------------------------------------------------------------------
# Model metadata for registration in MLJ...

MMI.metadata_model(PerceptronClassifier,
    input_scitype    = MMI.Table(MMI.Infinite),
    target_scitype   = AbstractVector{<: MMI.Finite},
    supports_weights = false,
    descr            = "The classical perceptron algorithm using one-vs-all for multiclass, from the Beta Machine Learning Toolkit (BetaML).",
	load_path        = "BetaML.Perceptron.PerceptronClassifier"
)

MMI.metadata_model(KernelPerceptronClassifier,
    input_scitype    = MMI.Table(MMI.Infinite),
    target_scitype   = AbstractVector{<: MMI.Finite},
    supports_weights = false,
    descr            = "The kernel perceptron algorithm using one-vs-one for multiclass, from the Beta Machine Learning Toolkit (BetaML).",
	load_path        = "BetaML.Perceptron.KernelPerceptronClassifier"
)

MMI.metadata_model(PegasosClassifier,
    input_scitype    = MMI.Table(MMI.Infinite),
    target_scitype   = AbstractVector{<: MMI.Finite},
    supports_weights = false,
    descr            = "The gradient-based linear \"pegasos\" classifier using one-vs-all for multiclass, from the Beta Machine Learning Toolkit (BetaML).",
	load_path        = "BetaML.Perceptron.PegasosClassifier"
)


@testset "construction and promotion" begin
    @test basetype(ZZ{Int}) == Int
    @test depth(ZZ{Int8}) == 1
    @test iscoprime(ZZ(18), ZZ(35))
    @test isirreducible(ZZ(17))
    @test !isirreducible(ZZ(18))
    @test typeof(copy(ZZ(big"2"))) == ZZ{BigInt}
    a = ZZ(16)
    @test ZZ{Int}(a) === a
    @test ZZ{Int8}(100) + ZZ(100) == ZZ(200)
    @test ZZ(10) + 10 == 20
    @test ZZ(1) + QQ(1, 2) == QQ(3, 2)
    @test ZZ(Int8(1)) + QQ(1, 2) == QQ(3, 2)
    @test typeof(ZZ(1) + QQ(1, 2)) == QQ{Int}
    @test typeof(ZZ(1) + 1//2) == QQ{Int}
    @test hash(ZZ(big"123")) == hash(123)
end

@testset "ZZ{$T}" for T in (Int32, Int64, BigInt)

    Z = ZZ{T}
    z = zero(Z)
    @test z == zero(z)
    @test iszero(z)
    @test !isone(z)
    o = one(z)
    @test isunit(o)
    @test isone(o)
    @test !iszero(o)
    n1 = T(19)
    n2 = Base.hastypemax(T) ? typemax(T) : T(big"987654321987654321987654321")
    z1 = Z(n1)
    z2 = ZZ(n2)
    @test !isunit(z1)
    @test z1 + z1 == ZZ(T(2n1))
    T != BigInt && @test_throws OverflowError z2 + z2

    @test z1 - z1 == z
    @test z1 - z2 == ZZ(n1 - n2)
    @test -z2 == ZZ(-n2)
    @test isunit(-o)
    T != BigInt && @test_throws OverflowError -z1 - z2

    @test z1 * z1 == ZZ(T(n1 * n1))
    @test z1 * n1 == ZZ(T(n1 * n1))
    @test n1 * z1 == ZZ(T(n1 * n1))
    T != BigInt && @test_throws OverflowError z1 * z2

    @test z1^2 == ZZ(n1^2)
    T != BigInt && @test_throws OverflowError z1^1000

    z3 = 2z1
    z4 = z3 + o
    @test z3 ÷ z1 == ZZ(T(2))
    @test z1 ÷ z3 == z
    @test z3 / z1 == ZZ(T(2))
    @test z1 \ z3 == ZZ(T(2))
    @test_throws DomainError z4 / z2

    @test inv(-o) == -o
    @test_throws DomainError inv(z1)

    @test gcd(z3, z1) == z1
    @test gcd(z3, z1, z3) == z1
    @test gcdx(z3, z1) == (z1, zero(z3), one(z1))
    @test gcdx(ZZ(60), ZZ(28), ZZ(6)) == (2, [-1, 2, 1])
    @test lcm(z3, z1) == z3
    @test rem(ZZ(12), ZZ(5)) == ZZ(2)

    @test sprint(show, z3) == "$(Int(2n1))"

    @test value(z2) === n2

    @test factor(z3) == [Z(2) => 1, n1 => 1]
    @test eltype(factor(z3)) == Pair{Z,Int}
end


# zwart_powell - compare Matlab and Julia versions

using MATLAB
using Sinograms: zwart_powell
using LazyGrids: ndgrid_array
#using Plots
#using MIRTjim: jim

# setup matlab to use MIRT
if !@isdefined(irtdir)
	ENV["MATLAB_ROOT"] = "/Applications/freeware/matlab"

	irtdir = "/Users/fessler/src/matlab/alg"
	tmp = "addpath('$irtdir')"
	eval_string(tmp)
	mat"setup"
end

r = LinRange(-1, 1, 2001) * 2
phi = LinRange(0, 2π, 361)
jsino = zwart_powell.(r, phi')

rr, pp = ndgrid_array(r, phi) # for matlab...
@mput rr
@mput pp
mat"msino = ir_radon_zwart_powell(pp, rr)"
@mget msino

@assert jsino ≈ msino

#jim(jim(jsino, "julia"), jim(msino, "matlab"), jim(jsino-msino, "diff"))


module BlockDiagonalFactors

using SparseArrays, SuiteSparse, LinearAlgebra

struct SparseBlockFactors{Tv}
    factors::Vector
    indices::Vector{<:Int}
    m::Int
    n::Int
end
struct BlockFactors{Tv}
    factors::Vector
    indices::Vector{<:Int}
    m::Int
    n::Int
end
export BlockFactors, SparseBlockFactors

# Factorization functions
for f in (:lu, :qr, :cholesky, :factorize), T in (:Float64, :(Complex{Float64}))
    @eval begin
        import LinearAlgebra: $f
        """
        $($f)(As::Vector{SparseMatrixCSC{$($T),Int}}, I::Vector{Int})

        Creates a block-diagonal (lazy) array of factors.
        Invokes `$($f)` on each matrix in the array of matrices `As` and stores them along with the indices `I`.
        """
        function $f(As::Vector{SparseMatrixCSC{$T,Int}}, I::Vector{Int})
            m = sum(i -> As[i].m, I)
            n = sum(i -> As[i].n, I)
            return SparseBlockFactors{$T}(map($f, As), I, m, n)
        end
        """
        $($f)(As::Vector{Array{$($T),2}}, I::Vector{Int})

        Creates a block-diagonal (lazy) array of factors.
        Invokes `$($f)` on each matrix in the array of matrices `As` and stores them along with the indices `I`.
        """
        function $f(As::Vector{Array{$T,2}}, I::Vector{Int})
            m = sum(i -> size(As[i],1), I)
            n = sum(i -> size(As[i],2), I)
            return BlockFactors{$T}(map($f, As), I, m, n)
        end
        export $f
    end
end

# TODO replace with lazy version? not sure how to do this
for f in (:adjoint, :transpose), TF in (:SparseBlockFactors, :BlockFactors), T in (:Float64, :(Complex{Float64}))
    @eval begin
        import Base: $f
        """
        $($f)(BDF::$($T))

        Invokes `$($f)` on all the factors in `BDF` and returns them into a new `$($T)` object.
        """
        $f(BDF::$TF{$T}) = $TF{$T}(map($f, BDF.factors), BDF.indices, BDF.m, BDF.n)
        export $f
    end
end

import Base.\
"""
    \\(BDF::SparseBlockFactors, y::AbstractVecOrMat)

Backsubstitution for `SparseBlockFactors`.
"""
function \(BDF::SparseBlockFactors{T}, y::AbstractVector{S}) where {T,S}
    x = Array{promote_type(T,S)}(undef, BDF.n)
    x_idx, y_idx = 0:0, 0:0
    my = size(y)[1]
    for i in BDF.indices
        x_idx = x_idx.stop .+ (1:size(BDF.factors[i],2))
        y_idx = y_idx.stop .+ (1:size(BDF.factors[i],1))
        x[x_idx] .= BDF.factors[i] \ y[y_idx]
    end
    return x
end
function \(BDF::SparseBlockFactors{T}, y::AbstractVector{T}) where T
    x = Array{T}(undef, BDF.n)
    x_idx, y_idx = 0:0, 0:0
    my = size(y)[1]
    for i in BDF.indices
        x_idx = x_idx.stop .+ (1:size(BDF.factors[i],2))
        y_idx = y_idx.stop .+ (1:size(BDF.factors[i],1))
        x[x_idx] .= BDF.factors[i] \ y[y_idx]
    end
    return x
end
"""
    \\(BDF::BlockFactors, y::AbstractVecOrMat)

Backsubstitution for `BlockFactors`.
"""
function \(BDF::BlockFactors{T}, y::AbstractVector{S}) where {T,S}
    x = Array{promote_type(T,S)}(undef, BDF.n)
    x_idx, y_idx = 0:0, 0:0
    my = size(y)[1]
    for i in BDF.indices
        x_idx = x_idx.stop .+ (1:size(BDF.factors[i],2))
        y_idx = y_idx.stop .+ (1:size(BDF.factors[i],1))
        x[x_idx] .= BDF.factors[i] \ y[y_idx]
    end
    return x
end
function \(BDF::BlockFactors{T}, y::AbstractVector{T}) where T
    x = Array{T}(undef, BDF.n)
    x_idx, y_idx = 0:0, 0:0
    my = size(y)[1]
    for i in BDF.indices
        x_idx = x_idx.stop .+ (1:size(BDF.factors[i],2))
        y_idx = y_idx.stop .+ (1:size(BDF.factors[i],1))
        x[x_idx] .= BDF.factors[i] \ y[y_idx]
    end
    return x
end
export \

import LinearAlgebra.ldiv!
"""
    ldiv!(BDF::SparseBlockFactors, y::AbstractVecOrMat)

Backsubstitution for `SparseBlockFactors`. For square blocks only.
"""
function ldiv!(BDF::SparseBlockFactors{T}, y::AbstractVector{T}) where T
    idx = 0:0
    for i in BDF.indices
        idx = idx.stop .+ (1:BDF.factors[i].n)
        y[idx] .= ldiv!(BDF.factors[i], y[idx])
    end
    return y
end
"""
    ldiv!(BDF::BlockFactors, y::AbstractVecOrMat)

Backsubstitution for `BlockFactors`. For square blocks only.
"""
function ldiv!(BDF::BlockFactors{T}, y::AbstractVector{T}) where T
    idx = 0:0
    for i in BDF.indices
        idx = idx.stop .+ (1:size(BDF.factors[i],2))
        y[idx] .= ldiv!(BDF.factors[i], y[idx])
    end
    return y
end
"""
    ldiv!(x::AbstractVecOrMat, BDF::SparseBlockFactors, y::AbstractVecOrMat)

Backsubstitution for `SparseBlockFactors`. For square blocks only.
"""
function ldiv!(x::AbstractVecOrMat{T}, BDF::SparseBlockFactors{T}, y::AbstractVector{T}) where T
    idx = 0:0
    for i in BDF.indices
        idx = idx.stop .+ (1:BDF.factors[i].n)
        x[idx] .= ldiv!(x[idx], BDF.factors[i], y[idx])
    end
    return x
end
"""
    ldiv!(x::AbstractVecOrMat, BDF::BlockFactors, y::AbstractVecOrMat)

Backsubstitution for `BlockFactors`. For square blocks only.
"""
function ldiv!(x::AbstractVecOrMat{T}, BDF::BlockFactors{T}, y::AbstractVector{T}) where T
    idx = 0:0
    for i in BDF.indices
        idx = idx.stop .+ (1:size(BDF.factors[i],2))
        x[idx] .= ldiv!(x[idx], BDF.factors[i], y[idx])
    end
    return x
end
export ldiv!

end # module


# log scale tests

scale_x_data = 10.0 .^ (-2:2)
scale_y_data = rand(5)

plot_1 = plot(scale_x_data, scale_y_data, title="plot 1")
yscale!(:log10)
xscale!(:log)

plot_2 = plot(
  scale_x_data, scale_y_data,
  xscale=:log10, yscale=:log
)

title!(plot_2, "plot 2")

test_is_same_object(plot_1, plot_2; except=["title"])

# lin scale tests

plot_A = plot(scale_x_data, scale_y_data, title="plot A")
xscale!(:log) ; yscale!(:linear)

plot_B = plot(
  scale_x_data, scale_y_data, xscale=:log10
)

title!("plot B")

test_is_same_object(plot_A, plot_B; except=["title"])
test_is_not_same_object(plot_A, plot_1; except=["title"])

# non-positive tests

plot(1:5, 1:5)
plot(1:5, 1:5, xscale=:log10, yscale=:log10)

plot(0:4, 1:5, xscale=:lin, yscale=:log10)
@test_throws AssertionError plot(0:4, 1:5, xscale=:log, yscale=:log10)

plot(1:5, -1:3, xscale=:log10, yscale=:identity)
@test_throws AssertionError plot(1:5, -1:3, xscale=:log10, yscale=:log10)


# N Queens Problem.

# (c) E. Adrian Garro S. Costa Rica Institute of Technology.

type NQueens
    queens_quant::Int
    solutions::Array{Array{Int}}
    solutions_num::Int
end

"""
`permutations(xs)`
Permutations without repeated elements.
"""
function permutations(xs)
    if xs == []
        produce(Int[])
    else
        for x in xs
            temp = copy(xs)
            filter!(e -> e != x, temp)
            for ys in @task permutations(temp)
                produce(union(Int[x], ys))
            end
        end
    end
end

function set_queens_quant!(problem::NQueens, queens_quant::Int)
    if queens_quant > 3
        problem.queens_quant = queens_quant
    else
        error("Invalid number for this problem.")
    end
end

function attack(queens_cols::Array{Int})
    queens_quant = length(queens_cols)
    queens_rows = 1:queens_quant
    queens_pos = zip(queens_rows, queens_cols)
    sum_of_pos = [sum(pos) for pos in queens_pos]
    negative_queens_cols = [-1 * queen_col for queen_col in queens_cols]
    queens_pos = zip(queens_rows, negative_queens_cols)
    diff_of_pos = [sum(pos) for pos in queens_pos]
    # all results are different?
    in_diag45 = length(sum_of_pos) > length(Set(sum_of_pos))
    in_diag135 = length(diff_of_pos) > length(Set(diff_of_pos))
    there_attack = in_diag45 || in_diag135
end

function not_attack(queens_cols::Array{Int})
    !attack(queens_cols)
end

function solve!(problem::NQueens)
    if problem.queens_quant != 0
        all_possible_queens_cols = @task permutations(
            collect(1:problem.queens_quant)
        )
        problem.solutions = collect(all_possible_queens_cols)
        filter!(not_attack, problem.solutions)
        problem.solutions_num = length(problem.solutions)
    else
       error("Number of queens not available.")
    end
end

function print_solution(problem::NQueens, solution_num::Int)
    if solution_num in collect(1:problem.solutions_num)
        solution = problem.solutions[solution_num]
        function create_box(queen_pos::Int)
            queen_pos -= 1
            string(
                "\n|",
                "   |" ^ queen_pos,
                " X |",
                "   |" ^ (problem.queens_quant - (queen_pos + 1))
            )
        end
        boxes = [create_box(queens_cols) for queens_cols in solution]
        chain = string("\n+", "---+"  ^ problem.queens_quant)
        chessboard = string(string(chain, join(boxes, chain)), chain)
        println(string("\nSolution #", solution_num, "\n"))
        println(chessboard)
    else
        error("Solution not exist.")
    end
end


# Test objective functions
# Author: Philipp Witte, pwitte3@gatech.edu
# Date: January 2020

using InvertibleNetworks, Test, LinearAlgebra

###################################################################################################
# MSE loss

Y = randn(Float32, 28, 28, 3, 1)
X = randn(Float32, 28, 28, 3, 1)
X0 = randn(Float32, 28, 28, 3, 1)
dX = X - X0

function loss_mse(X, Y)
    f = mse(X, Y)
    g = ∇mse(X, Y)
    return f, g
end

f0, g = loss_mse(X0, Y)
h = .1f0
maxiter = 6

err1 = zeros(Float32, maxiter)
err2 = zeros(Float32, maxiter)

print("\nGradient test mse loss\n")
for j=1:maxiter
    f = loss_mse(X0 + h*dX, Y)[1]
    err1[j] = abs(f - f0)
    err2[j] = abs(f - f0 - h*dot(dX, g))
    print(err1[j], "; ", err2[j], "\n")
    global h = h/2f0
end

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

###################################################################################################
# (Negative) Log-likelihood

X = randn(Float32, 28, 28, 3, 1)
X0 = randn(Float32, 28, 28, 3, 1)
dX = X - X0

function loss_mle(X)
    f = log_likelihood(X; μ=1f0, σ=2f0)
    g = ∇log_likelihood(X; μ=1f0, σ=2f0)
    return f, g
end

f0, g = loss_mle(X0)
h = .1f0
maxiter = 6

err3 = zeros(Float32, maxiter)
err4 = zeros(Float32, maxiter)

print("\nGradient test log likelihood\n")
for j=1:maxiter
    f = loss_mle(X0 + h*dX)[1]
    err3[j] = abs(f - f0)
    err4[j] = abs(f - f0 - h*dot(dX, g))
    print(err3[j], "; ", err4[j], "\n")
    global h = h/2f0
end

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


using SparseArrays
using GraphNeuralNetworks
using BenchmarkTools
import Random: seed!
using LinearAlgebra

n = 1024
seed!(0)
A = sprand(n, n, 0.01)
b = rand(1, n)
B = rand(100, n)

g = GNNGraph(
    A,
    ndata=(; b=b, B=B),
    edata=(; A=reshape(A.nzval, 1, :)),
    graph_type=:coo  # changing to :sparse has little effect on performance
)

function spmv(g)
    propagate(
        (xi, xj, e) -> e .* xj ,  # same as e_mul_xj
        g, +; xj=g.ndata.b, e=g.edata.A
        )
end

function spmm1(g)
    propagate(
        (xi, xj, e) -> e .* xj ,  # same as e_mul_xj
        g, +; xj=g.ndata.B, e=g.edata.A
        )
end
function spmm2(g)
    propagate(
        e_mul_xj,
        g, +; xj=g.ndata.B, e=vec(g.edata.A)
        )
end

# @assert isequal(spmv(g),  b * A)  # true
# @btime spmv(g)  # ~5 ms
# @btime b * A  # ~32 us

@assert isequal(spmm1(g), B * A)  # true
@assert isequal(spmm2(g), B * A)  # true
@btime spmm1(g)  # ~9 ms
@btime spmm2(g)  # ~9 ms
@btime B * A  # ~400 us

function spmm_copyxj_fused(g)
    propagate(
        copy_xj,
        g, +; xj=g.ndata.B
        )
end

function spmm_copyxj_unfused(g)
    propagate(
        (xi, xj, e) -> xj,
        g, +; xj=g.ndata.B
        )
end

Adj = map(x -> x > 0 ? 1 : 0, A)
@assert spmm_copyxj_unfused(g) ≈ B * Adj
@assert spmm_copyxj_fused(g) ≈ B * Adj # bug fixed in #107

@btime spmm_copyxj_fused(g)  # 268.614 μs (22 allocations: 1.13 MiB)
@btime spmm_copyxj_unfused(g)  # 4.263 ms (52855 allocations: 12.23 MiB)
@btime B * Adj  # 196.135 μs (2 allocations: 800.05 KiB)

println()


#Demo 5: Fourier Series
#-------------------------------------------------------------------------------

using CMDimCircuits
CMDimCircuits.@using_CData()

#Get a demo display:
include(CMDimCircuits.demoplotcfgscript)
#Normally use something like:
#CMDimData.@includepkg EasyPlotInspect

#==Constants
===============================================================================#
vvst = cons(:a, labels = set(yaxis="Amplitude (V)", xaxis="Time (s)"))
magvsf = cons(:a, labels = set(yaxis="|X(f)|²", xaxis="Frequency (Hz)"))

#==Input data
===============================================================================#
tfund = 1e-9 #Fundamental
osr = 20 #(fundamental) oversampling ratio
nfund = 20 #cycles of the fundamenal

#==Computations
===============================================================================#
t = DataTime(timespace(:ts, tfund/osr, :tfund, nfund*tfund), tperiodic=true)
sig = sin(t*(2pi/tfund))
Fsig = freqdomain(sig)
#sigSpec = abs2(fspectrum(Fsig)) #Aperiodic
sigSpec = abs2(fcoeff(Fsig)) #Periodic

#==Generate plot
===============================================================================#
plot_t = push!(cons(:plot, vvst, title="Time domain", legend=false),
#	cons(:wfrm, DataF1(sig), glyph=set(shape=:o)),
	cons(:wfrm, DataF1(sig)),
)
plot_f = push!(cons(:plot, magvsf, title="Frequency domain", legend=false),
	cons(:wfrm, sigSpec, glyph=set(shape=:o, size=1.5), line=set(style=:none, color=:red, width=2)),
)

pcoll = push!(cons(:plotcoll, title="Fourier Transform Tests"), plot_t, plot_f)
	pcoll.ncolumns = 1

#==Display results in pcoll
===============================================================================#
EasyPlot.displaygui(pcoll)

#==Return pcoll to user (call evalfile(...))
===============================================================================#
pcoll


@testset "gainobj" begin

box_n = Cuboid(-[0.5,0.5,0.5],[1,1,1])
gbox_n = GainObject{3,3}(box_n, d::Real->-d)
@test (d = rand(); gbox_n.D₀fun(d) == -d)

box_p = Cuboid([0.5,0.5,0.5],[1,1,1])
gbox_p = GainObject{3,3}(box_p, d::Real->d)
@test (d = rand(); gbox_p.D₀fun(d) == d)

gobj_vec = [gbox_n, gbox_p]

# grid: from [-2,-2,-2] to [2,2,2] with ∆ = 1.  Exclude ghost points.
l = (([-2,-1,0,1],[-2,-1,0,1],[-2,-1,0,1]), ([-1.5,-0.5,0.5,1.5],[-1.5,-0.5,0.5,1.5],[-1.5,-0.5,0.5,1.5]))
N = SVector(4,4,4)

D₀ = zeros(3*prod(N))
d = 1.0
gt₀ = @SVector [PRIM,PRIM,PRIM]
assign_pumpstr!(D₀, gobj_vec, d, gt₀, l, false, true)

# Constrcut the expected D₀.
D₀exp = zeros(3, N...)

# x-components
D₀exp[1, 2, 2:3, 2:3] .= -1.0  # gbox_n
D₀exp[1, 3, 3:4, 3:4] .= 1.0  # gbox_p

# y-components
D₀exp[2, 2:3, 2, 2:3] .= -1.0  # gbox_n
D₀exp[2, 3:4, 3, 3:4] .= 1.0  # gbox_p

# z-components
D₀exp[3, 2:3, 2:3, 2] .= -1.0  # gbox_n
D₀exp[3, 3:4, 3:4, 3] .= 1.0  # gbox_p

@test D₀exp[:] == D₀

end  # @testset "gainobj"


using LBLRTM
using Unitful
using Test

@testset "Planck function and its inverse" begin
    ν = 1000u"cm^-1"
    T_0 = 300u"K"
    L = planck(ν, T_0)
    T_1 = radiance_to_bt(ν, L)
    T_2 = radiance_to_bt(ν)(L)

    @test T_0 == T_1
    @test T_0 == T_2
end


immutable ITCH{T} <: Model
    β₀::T
    β₁::T
    β₂::T
    β₃::T
    β₄::T
end

Base.show(io::IO, ::Type{ITCH}) = print(io, "Heuristic ITCH")

function Base.show(io::IO, m::ITCH)
    @printf(
        io,
        "Heuristic ITCH (β₀ = %.3f, β₁ = %.3f, β₂ = %.3f, β₃ = %.3f, β₄ = %.3f)",
        m.β₀,
        m.β₁,
        m.β₂,
        m.β₃,
        m.β₄,
    )
end

initial_parameters(::Type{ITCH}) = Float64[0.0, 0.0, 0.0, 0.0, 0.0]

canonical_parameters(m::ITCH) = (m.β₀, m.β₁, m.β₂, m.β₃, m.β₄)

function ITCH(parameters::Vector)
    β₀, β₁, β₂, β₃, β₄ = (
        parameters[1],
        parameters[2],
        parameters[3],
        parameters[4],
        parameters[5],
    )
    return ITCH(β₀, β₁, β₂, β₃, β₄)
end

@inline function predict(m::ITCH, x2::Real, t2::Real, x1::Real, t1::Real)
    β₀, β₁, β₂, β₃, β₄ = m.β₀, m.β₁, m.β₂, m.β₃, m.β₄
    δx = x2 - x1
    δt = t2 - t1
    z = (
        β₀ +
        β₁ * δx +
        β₂ * (δx / ((x1 + x2) / 2)) +
        β₃ * δt +
        β₄ * (δt / ((t1 + t2) / 2))
    )
    p = invlogit(z)
    return p
end

@inline function gradient_component(
    m::ITCH,
    x2::Real,
    t2::Real,
    x1::Real,
    t1::Real,
)
    δx = x2 - x1
    δt = t2 - t1
    gr = (
        1.0,
        δx,
        (δx / ((x1 + x2) / 2)),
        δt,
        (δt / ((t1 + t2) / 2)),
    )
    return gr
end


mutable struct Board
    values::Array{Int}
    marked::BitArray
    #marked = iszero.(zeros((5,5)))
end

function step(instruction, board)
    for (coord, val) in enumerate(board.values)
        if val == instruction
            board.marked[coord] = true
        end
    end
end

function check(board)
    for row in eachrow(board.marked)
        if all(row .== true)
            return true
        end
    end
    for col in eachcol(board.marked)
        if all(col .== true)
            return true
        end
    end
    return false
end

function computeScore(board, instruction)
    score = 0
    for (coord, val) in enumerate(board.values)
        if board.marked[coord] == false
            score += val
        end
    end
    println("sum: "*string(score))
    println("instruction: "*string(instruction))
    return score * instruction
end

function solve(filename)
    #load txt file
    values = readlines(filename*".txt")
    #first line is instructions
    instructions = values[1]
    instructions = split(instructions,',')
    instructions = parse.(Int, instructions)
    #next line is empty -> board from line 3-7
    start = 3
    stop = 7
    boards = Board[]
    while stop <= length(values)
        boardstring = values[start:stop]
        intboard = fill(0, (5,5))
        i = 1
        for row in boardstring
            row = split(row, ' ')
            row = filter(!isempty, row)
            introw = parse.(Int, row)
            for (j, val) in enumerate(introw)
                intboard[i,j] = val
            end
            i += 1
        end
        marked = iszero.(ones((5,5)))
        board = Board(copy(intboard), copy(marked))
        boards = push!(boards, board)
        start += 6
        stop += 6
    end
    score = nothing
    for ins in instructions
        for b in boards
            step(ins, b)
            if check(b) == true
                score = computeScore(b, ins)
                println("Winning score: "*string(score))
                break
            end
        end
        if !isnothing(score)
            break
        end
    end

end

solve("input")
solve("smalltest")

function solve2(filename)
    #load txt file
    values = readlines(filename*".txt")
    #first line is instructions
    instructions = values[1]
    instructions = split(instructions,',')
    instructions = parse.(Int, instructions)
    #next line is empty -> board from line 3-7
    start = 3
    stop = 7
    boards = Board[]
    while stop <= length(values)
        boardstring = values[start:stop]
        intboard = fill(0, (5,5))
        i = 1
        for row in boardstring
            row = split(row, ' ')
            row = filter(!isempty, row)
            introw = parse.(Int, row)
            for (j, val) in enumerate(introw)
                intboard[i,j] = val
            end
            i += 1
        end
        marked = iszero.(ones((5,5)))
        board = Board(copy(intboard), copy(marked))
        boards = push!(boards, board)
        start += 6
        stop += 6
    end
    scores = Int[]
    winningboards = Board[]
    score = nothing
    boardsMarked = zeros(length(boards))
    for ins in instructions
        for (j,b) in enumerate(boards)
            if boardsMarked[j] == 0
                step(ins, b)
                if check(b) == true
                    score = computeScore(b, ins)
                    winningboards = push!(winningboards, b)
                    scores = push!(scores, score)
                    boardsMarked[j] = 1
                end
            end
        end
    end
    println("Winning score: "*string(score))
    println("All scores: ")
    println(scores)
end

solve2("smalltest")
solve2("input")


using ArbNumerics
using Test

setprecision(BigFloat, 512)

include("arithmetic.jl")


# Tests for validated_integ

using TaylorModels
# using LinearAlgebra: norm
using Test
using Random

const _num_tests = 1_000

setformat(:full)

# NOTE: IntervalArithmetic v0.16.0 includes this function; but
# IntervalRootFinding is bounded to use v0.15.x
interval_rand(X::Interval{T}) where {T} = X.lo + rand(T) * (X.hi - X.lo)
interval_rand(X::IntervalBox) = interval_rand.(X)

# Function to check, against an exact solution of the ODE, the computed
# validted solution
function test_integ(fexact, t0, qTM, q0, δq0)
    normalized_box = symmetric_box(length(q0), Float64)
    # Time domain
    domt = domain(qTM[1])
    # Random time (within time domain) and random initial condition
    δt = rand(domt)
    δtI = (δt .. δt) ∩ domt
    q0ξ = interval_rand(δq0)
    q0ξB = IntervalBox([(q0ξ[i] .. q0ξ[i]) ∩ δq0[i] for i in eachindex(q0ξ)])
    # Box computed to overapproximate the solution at time δt
    q = evaluate.(evaluate.(qTM, δtI), (normalized_box,))
    # Box computed from the exact solution must be within q
    bb = all(fexact(t0+δtI, q0 .+ q0ξB) .⊆ q)
    # Display details if bb is false
    bb || @show(t0, domt, remainder.(qTM),
            δt, δtI, q0ξ, q0ξB, q,
            fexact(t0+δtI, q0 .+ q0ξB))
    return bb
end

@testset "Tests for `validated_integ`" begin
    @testset "falling_ball!" begin
        @taylorize function falling_ball!(dx, x, p, t)
            dx[1] = x[2]
            dx[2] = -one(x[1])
            nothing
        end
        exactsol(t, t0, x0) = (x0[1] + x0[2]*(t-t0) - 0.5*(t-t0)^2, x0[2] - (t-t0))

        # Initial conditions
        tini, tend = 0.0, 10.0
        normalized_box = symmetric_box(2, Float64)
        q0 = [10.0, 0.0]
        δq0 = 0.25 * normalized_box
        X0 = IntervalBox(q0 .+ δq0)

        # Parameters
        abstol = 1e-20
        orderQ = 2
        orderT = 4
        ξ = set_variables("ξₓ ξᵥ", order=2*orderQ, numvars=length(q0))

        @testset "Forward integration 1" begin
            sol = validated_integ(falling_ball!, X0, tini, tend, orderQ, orderT, abstol)
            tTM = expansion_point(sol)
            qv  = flowpipe(sol)
            qTM = get_xTM(sol)
            @test length(qv) == length(qTM[1,:]) == length(sol)
            @test firstindex(sol) == 1
            @test sol[2] == get_xTM(sol,2)
            @test domain(sol,1) == 0..0

            end_idx = lastindex(sol)
            Random.seed!(1)
            for it = 1:_num_tests
                n = rand(2:end_idx)
                @test test_integ((t,x)->exactsol(t,tini,x), tTM[n], sol[n], q0, δq0)
            end

            solf = validated_integ(falling_ball!, X0, tini, tend, orderQ, orderT, abstol,
                adaptive=false)
            qvf, qTMf = getfield.((solf,), 2:3)

            @test length(qvf) == length(qv)
            @test qTM == qTMf

            # initializaton with a Taylor model
            X0tm = sol[1]
            sol2 = validated_integ(falling_ball!, X0tm, tini, tend, orderQ, orderT, abstol)
            qTM2 = getfield(sol2, 3)
            @test qTM == qTM2
            @test sol2[1,2] == X0tm[2]

            # Tests for TaylorModels.mince_in_time
            domT = TaylorModels.mince_in_time(sol)
            @test domT == expansion_point(sol) .+ domain(sol)
            timesdiv = TaylorModels.mince_in_time(sol, var=0, timediv=2)
            fpdiv = TaylorModels.mince_in_time(sol, var=1, timediv=2)
            @test timesdiv[3] ⊂ domT[2]
            @test hull(timesdiv[1],timesdiv[2]) == domT[1]
            @test fpdiv[3] ⊂ qv[2][1]
            @test hull(fpdiv[3],fpdiv[4]) ⊂ qv[2][1]
        end

        @testset "Forward integration 2" begin
            sol = validated_integ2(falling_ball!, X0, tini, tend, orderQ, orderT, abstol)
            tTM = expansion_point(sol)
            qv  = flowpipe(sol)
            qTM = get_xTM(sol)
            @test length(qv) == length(qTM[1,:]) == length(sol)
            @test firstindex(sol) == 1
            @test sol[2] == get_xTM(sol,2)
            @test domain(sol,1) == 0..0

            Random.seed!(1)
            end_idx = lastindex(sol)
            for it = 1:_num_tests
                n = rand(2:end_idx)
                @test test_integ((t,x)->exactsol(t,tini,x), tTM[n], sol[n], q0, δq0)
            end

            # initializaton with a Taylor model
            X0tm = get_xTM(sol,1)
            sol2 = validated_integ2(falling_ball!, X0tm, tini, tend, orderQ, orderT, abstol)
            qTM2 = getfield(sol2, 3)
            @test qTM == qTM2
            @test sol2[1,2] == X0tm[2]
        end

        # Initial conditions
        tini, tend = 10.0, 0.0
        q0 = [10.0, 0.0]
        δq0 = IntervalBox(-0.25 .. 0.25, 2)
        X0 = IntervalBox(q0 .+ δq0)

        @testset "Backward integration 1" begin
            sol = validated_integ(falling_ball!, X0, tini, tend, orderQ, orderT, abstol)
            tTM, qv, qTM = getfield.((sol,), 1:3)
            @test length(qv) == length(qTM[1,:]) == length(sol)
            @test firstindex(sol) == 1
            @test sol[2] == get_xTM(sol,2)
            @test domain(sol,1) == 0..0

            Random.seed!(1)
            end_idx = lastindex(sol)
            for it = 1:_num_tests
                n = rand(2:end_idx)
                @test test_integ((t,x)->exactsol(t,tini,x), expansion_point(sol,n), sol[n], q0, δq0)
            end

            solf = validated_integ(falling_ball!, X0, tini, tend, orderQ, orderT, abstol,
                adaptive=false)
            tTMf, qvf, qTMf = getfield.((solf,), 1:3)

            @test length(qvf) == length(qv)
            @test all(qTM .== qTMf)

            # initializaton with a Taylor model
            X0tm = sol[1]
            sol2 = validated_integ(falling_ball!, X0tm, tini, tend, orderQ, orderT, abstol)
            qv2, qTM2 = getfield.((sol2,), 2:3)
            @test qTM == qTM2

            sol2f = validated_integ(falling_ball!, X0tm, tini, tend, orderQ, orderT, abstol,
                adaptive=false)
            qv2f, qTM2f = getfield.((sol2f,), 2:3)
            @test length(qv2f) == length(qv2)
            @test all(qTM .== qTM2f)
        end

        @testset "Backward integration 2" begin
            sol = validated_integ2(falling_ball!, X0, tini, tend, orderQ, orderT, abstol)
            tTM, qv, qTM = getfield.((sol,), 1:3)
            @test length(qv) == length(qTM[1,:]) == length(sol)
            @test firstindex(sol) == 1
            @test sol[2] == get_xTM(sol,2)
            @test domain(sol,1) == 0..0

            Random.seed!(1)
            end_idx = lastindex(sol)
            for it = 1:_num_tests
                n = rand(2:end_idx)
                @test test_integ((t,x)->exactsol(t,tini,x), expansion_point(sol,n), sol[n], q0, δq0)
            end
        end
    end

    @testset "x_square!" begin
        @taylorize function x_square!(dx, x, p, t)
            dx[1] = x[1]^2
            nothing
        end

        exactsol(t, x0) = 1 / (1/x0[1] - t)

        tini, tend = 0., 0.45
        normalized_box = symmetric_box(1, Float64)
        abstol = 1e-15
        orderQ = 5
        orderT = 20
        q0 = [2.]
        δq0 = 0.0625 * normalized_box
        X0 = IntervalBox(q0 .+ δq0)
        ξ = set_variables("ξₓ", numvars=1, order=2*orderQ)

        @testset "Forward integration 1" begin
            sol = validated_integ(x_square!, X0, tini, tend, orderQ, orderT, abstol)
            tTM, qv, qTM = getfield.((sol,), 1:3)
            @test length(qv) == length(qTM[1, :]) == length(tTM)
            @test domain(sol,1) == 0..0

            Random.seed!(1)
            end_idx = lastindex(tTM)
            for it = 1:_num_tests
                n = rand(1:end_idx)
                @test test_integ((t,x)->exactsol(t,x), tTM[n], sol[n], q0, δq0)
            end

            solf = validated_integ(x_square!, X0, tini, tend, orderQ, orderT, abstol,
                adaptive=false)
            tTMf, qvf, qTMf = getfield.((solf,), 1:3)

            @test length(qvf) == length(qv)
            @test all(qTMf .== qTM)

            # initializaton with a Taylor model
            X0tm = copy(qTM[:, 1])
            sol2 = validated_integ(x_square!, X0tm, tini, tend, orderQ, orderT, abstol)
            tTM2, qv2, qTM2 = getfield.((sol2,), 1:3)
            @test qTM == qTM2
        end

        @testset "Forward integration 2" begin
            sol = validated_integ2(x_square!, X0, tini, tend, orderQ, orderT, abstol)
            tTM, qv, qTM = getfield.((sol,), 1:3)
            @test domain(sol,1) == 0..0

            @test length(qv) == length(qTM[1, :]) == length(tTM)

            Random.seed!(1)
            end_idx = lastindex(tTM)
            for it = 1:_num_tests
                n = rand(1:end_idx)
                @test test_integ((t,x)->exactsol(t,x), tTM[n], sol[n], q0, δq0)
            end
        end
    end

    @testset "Pendulum with constant torque" begin
        @taylorize function pendulum!(dx, x, p, t)
            si = sin(x[1])
            aux = 2 *  si
            dx[1] = x[2]
            dx[2] = aux + 8*x[3]
            dx[3] = zero(x[1])
            nothing
        end
        # Conserved quantity
        ene_pendulum(x) = x[2]^2/2 + 2 * cos(x[1]) - 8 * x[3]

        # Initial conditions
        tini, tend = 0.0, 12.0
        q0 = [1.1, 0.1, 0.0]
        δq0 = IntervalBox(-0.1 .. 0.1, -0.1 .. 0.1, 0..0)
        X0 = IntervalBox(q0 .+ δq0)
        ene0 = ene_pendulum(X0)

        # Parameters
        abstol = 1e-10
        orderQ = 3
        orderT = 10
        ξ = set_variables("ξ", order=2*orderQ, numvars=length(q0))

        sol = validated_integ(pendulum!, X0, tini, tend, orderQ, orderT, abstol);
        @test all(ene0 .⊆ ene_pendulum.(flowpipe(sol)))

        sol = validated_integ2(pendulum!, X0, tini, tend, orderQ, orderT, abstol,
            validatesteps=32);
        @test all(ene0 .⊆ ene_pendulum.(flowpipe(sol)))

        # Initial conditions 2
        q0 = [1.1, 0.1, 0.0]
        δq0 = IntervalBox(-0.1 .. 0.1, -0.1 .. 0.1, -0.01 .. 0.01)
        X0 = IntervalBox(q0 .+ δq0)
        ene0 = ene_pendulum(X0)

        sol = validated_integ(pendulum!, X0, tini, tend, orderQ, orderT, abstol);
        @test all(ene0 .⊆ ene_pendulum.(flowpipe(sol)))

        sol = validated_integ2(pendulum!, X0, tini, tend, orderQ, orderT, abstol,
            validatesteps=32);
        @test all(ene0 .⊆ ene_pendulum.(flowpipe(sol)))
    end
end


@testset "QP Example from Maros" begin
  @testset "parseqps($T, filename)" for T in subtypes(AbstractFloat)
    # MPS description
    mps   = parseqps(T, joinpath(dirname(@__FILE__), "qp-example.qps"))

    @test name(mps) == "QP example"

    @test variables(mps) == [:c1, :c2]

    Q, q₁, q₂ = objective(mps)
    @test Q ≈ [8 2; 2 10] && eltype(Q) == T
    @test q₁ ≈ [1.5; -2] && eltype(q₁) == T
    @test q₂ ≈ 4 && eltype(q₂) == T

    A, b = equalities(mps)
    @test isempty(A) == true && eltype(A) == T
    @test isempty(b) == true && eltype(b) == T

    C, c₁, c₂ = inequalities(mps)
    @test C ≈ [2 1; -1 2] && eltype(C) == T
    @test c₁ ≈ [2; -Inf] && eltype(c₁) == T
    @test c₂ ≈ [Inf, 6] && eltype(c₂) == T

    lb, ub = bounds(mps)
    @test lb ≈ zeros(2) && eltype(lb) == T
    @test ub ≈ [20, Inf] && eltype(ub) == T
    # Canonical description
    canon = canonical(mps)

    @test name(canon) == name(mps)

    @test variables(canon) == variables(mps)

    QQ, qq1, qq2 = objective(canon)
    @test QQ ≈ Q && qq1 ≈ q₁ && qq2 ≈ q₂

    AA, bb = equalities(canon)
    @test AA ≈ A && bb ≈ b

    C̃, c̃ = inequalities(canon)
    @test C̃ ≈ [-2 -1; -1 2] && eltype(C̃) == T
    @test c̃ ≈ [-2; 6] && eltype(c̃) == T

    llb, uub = bounds(canon)
    @test llb ≈ lb && uub ≈ ub
  end

  @testset "parse_sparseqps($T, filename)" for T in subtypes(AbstractFloat)
    # MPS description
    mps   = parse_sparseqps(T, joinpath(dirname(@__FILE__), "qp-example.qps"))

    @test name(mps) == "QP example"

    @test variables(mps) == [:c1, :c2]

    Q, q₁, q₂ = objective(mps)
    @test typeof(Q) == SparseMatrixCSC{T,Int} && Matrix(Q) ≈ [8 2; 2 10]
    @test q₁ ≈ [1.5; -2] && eltype(q₁) == T
    @test q₂ ≈ 4 && eltype(q₂) == T

    A, b = equalities(mps)
    @test typeof(A) == SparseMatrixCSC{T,Int} && isempty(A) == true
    @test isempty(b) == true && eltype(b) == T

    C, c₁, c₂ = inequalities(mps)
    @test typeof(C) == SparseMatrixCSC{T,Int} && Matrix(C) ≈ [2 1; -1 2]
    @test c₁ ≈ [2; -Inf] && eltype(c₁) == T
    @test c₂ ≈ [Inf, 6] && eltype(c₂) == T

    lb, ub = bounds(mps)
    @test lb ≈ zeros(2) && eltype(lb) == T
    @test ub ≈ [20, Inf] && eltype(ub) == T
    # Canonical description
    canon = canonical(mps)

    @test name(canon) == name(mps)

    @test variables(canon) == variables(mps)

    QQ, qq1, qq2 = objective(canon)
    @test QQ ≈ Q && qq1 ≈ q₁ && qq2 ≈ q₂

    AA, bb = equalities(canon)
    @test AA ≈ A && bb ≈ b

    C̃, c̃ = inequalities(canon)
    @test typeof(C̃) == SparseMatrixCSC{T,Int} && Matrix(C̃) ≈ [-2 -1; -1 2]
    @test c̃ ≈ [-2; 6] && eltype(c̃) == T

    llb, uub = bounds(canon)
    @test llb ≈ lb && uub ≈ ub
  end
end


## test DoublePendulum after creating a joint between two single pendulums
# include("../src/plotSol.jl")
include("../src/plotSol.jl")
include("../src/simulateDiff.jl")
include("../src/OrientationConversion.jl")
using Revise
using JLD
using BenchmarkTools
##
clearconsole()
j1 = Nothing
j2 = Nothing
## Pendulum 1
m = 1.0; l = 1.0; # Mass and length of bar
# Assuming bar revolves about Y axis
I1 = [1 0 0; 0 m*l^2/12 0; 0 0 1]

# Testing Dynamics with Revolute Joint
Pend1 = RigidBody(m,I1,2)
RbI = InertialFrameAsRB()
x0R1 = [l/2;zeros(2);[1;zeros(3)];zeros(3);zeros(4)]
initialiseRigidBody!(Pend1,x0R1)

axisY = [0.0 1.0 0.0][:] # Axis about which bar is revolving

rj1 = [0.0 0.0 0.0][:] # Joint Location in body frame of first body
rj2 = [-l/2 0.0 0.0][:] # Joint location in body frame of second body

j1_1 = Joint(RbI,Pend1,rj1,rj2,type="Revolute",axis=axisY)
j1 = [j1_1];

# just simulate without any external force or gravity first
g = zeros(3);
tEnd = 1.0; tInt = 0.01;
tSim, solFinal = simulateDiff(tEnd,tInt,j1,g=g);
solPend1 = solFinal[1];

## Pendulum 2
Pend2 = RigidBody(m,I1,2);
x0R2 = [3*l/2;zeros(2);[1;zeros(3)];zeros(3);zeros(4)]
initialiseRigidBody!(Pend2,x0R2)

rj1 = [l 0.0 0.0][:] #Joint location wrt inertial frame
rj2 = [-l/2 0.0 0.0][:] #Joint location in body frame of pend2

j2_1 = Joint(RbI,Pend2,rj1,rj2,type="Revolute",axis=axisY);
j2 = [j2_1];

# just simulate without any external force or gravity first
g = zeros(3);
tEnd = 1.0; tInt = 0.01;
tSim, solFinal = simulateDiff(tEnd,tInt,j1,g = g);
solPend2 = solFinal[1];

## Join the two pendulums
println("########################")
println("Joining the two pendulums")
rj1 = [l/2;zeros(2)];
rj2 = [-l/2;zeros(2)];
joinBodies!(j1,j2,2,2,rj1,rj2, type = "Revolute", axis = axisY);

## Simulation
g = [0.0,0.0,-9.806]; # Gravity Vector.
tEnd = 1.0
tSpan = 0.01
tSim, solFinal = simulateDiff(tEnd,tSpan,j1,g=g)

solPend1  = solFinal[1]
solPend2  = solFinal[2]
##
# Check if joint location has moved
jointLoc1 = Matrix{Float64}(undef,length(tSim),3)
jointLoc2 = Matrix{Float64}(undef,length(tSim),3)
ωSol = Matrix{Float64}(undef,length(tSim),3)
for i=1:length(tSim)
    Pend1.dcm = quat2dcm(solPend1.β[i,:])
    Pend2.dcm = quat2dcm(solPend2.β[i,:])
    # ωSol[i,:] = angVel(solPend1.β[i,:],solPend1.βdot[i,:])
    jointLoc1[i,:] = solPend1.r[i,:] + transpose(Pend1.dcm)*rj1
    jointLoc2[i,:] = solPend2.r[i,:] + transpose(Pend2.dcm)*rj2
end
##
close("all");
# plotErrNorm(tSim,solPend.β)
plotPos(tSim,jointLoc1 .- jointLoc2)
# plotPos(tSim,jointLoc2)
# plotQuat(tSim, solPend1.β)
# plotAngVel(tSim, ωSol)
# plotPos(tSim, solPend.r)
## Compare with MinReal
solMin = load("DoublePendulumMin.jld")
r1Min = solMin["r1Min"]
r2Min = solMin["r2Min"]

# Plotting errors
plotPos(tSim,r1Min-solPend1.r)
plotPos(tSim,r2Min-solPend2.r)
# plotAngVel(tSim,ωMin+ωSol)
# plotPos(tSim,solPend.r)


import Base.reshape, Base.vec
export reshape, vec
export sign, curvature, monotonicity, evaluate, conic_form!

struct ReshapeAtom <: AbstractExpr
    head::Symbol
    id_hash::UInt64
    children::Tuple{AbstractExpr}
    size::Tuple{Int, Int}

    function ReshapeAtom(x::AbstractExpr, m::Int, n::Int)
        if m * n != length(x)
            error("Cannot reshape expression of size $(x.size) to ($(m), $(n))")
        end
        return new(:reshape, objectid(x), (x,), (m, n))
    end
end

function sign(x::ReshapeAtom)
    return sign(x.children[1])
end

function monotonicity(x::ReshapeAtom)
    return (Nondecreasing(),)
end

function curvature(x::ReshapeAtom)
    return ConstVexity()
end

function evaluate(x::ReshapeAtom)
    return reshape(evaluate(x.children[1]), x.size[1], x.size[2])
end

function conic_form!(x::ReshapeAtom, unique_conic_forms::UniqueConicForms)
    return conic_form!(x.children[1], unique_conic_forms)
end

reshape(x::AbstractExpr, m::Int, n::Int) = ReshapeAtom(x, m, n)
vec(x::AbstractExpr) = reshape(x, length(x), 1)


@testset "limiters" begin
	@test DiscretisedFluidQueues.minmod(0.0,0.0,0.0)==0.0
	@test DiscretisedFluidQueues.minmod(0.0,0.0,1.0)==0.0
	@test DiscretisedFluidQueues.minmod(0.0,1.0,1.0)==0.0
	@test DiscretisedFluidQueues.minmod(0.1,1.0,1.0)==0.1
	@test DiscretisedFluidQueues.minmod(-1.0,1.0,1.0)==0.0
	@test DiscretisedFluidQueues.minmod(-1.0,-1.0,-0.1)==-0.1
	@test DiscretisedFluidQueues.minmod(-1.0,-1.0,0.0)==0.0
	@test DiscretisedFluidQueues.minmod(-1.0,1.0,0.0)==0.0

	V2 = DiscretisedFluidQueues.vandermonde(2)
	linear_coeffs2 = [0.0;1.0]
	delta = 1.0
	@test isapprox(DiscretisedFluidQueues.linear(linear_coeffs2,V2...,delta)[1],linear_coeffs2)
	@test isapprox(DiscretisedFluidQueues.linear(linear_coeffs2,V2...,delta)[2],2.0/delta^2)
	delta = 0.25
	@test isapprox(DiscretisedFluidQueues.linear(linear_coeffs2,V2...,delta)[1],linear_coeffs2)
	@test isapprox(DiscretisedFluidQueues.linear(linear_coeffs2,V2...,delta)[2],2.0/delta^2)
	linear_coeffs2 = [0.5;0.5]
	delta = 1.0
	@test isapprox(DiscretisedFluidQueues.linear(linear_coeffs2,V2...,delta)[1],linear_coeffs2)
	@test isapprox(DiscretisedFluidQueues.linear(linear_coeffs2,V2...,delta)[2],0.0)
	delta = 0.25
	@test isapprox(DiscretisedFluidQueues.linear(linear_coeffs2,V2...,delta)[1],linear_coeffs2)
	@test isapprox(DiscretisedFluidQueues.linear(linear_coeffs2,V2...,delta)[2],0.0)

	V3 = DiscretisedFluidQueues.vandermonde(3)
	linear_coeffs3 = [1/6; 2/3; 1/6]
	delta = 1.0
	@test isapprox(DiscretisedFluidQueues.linear(linear_coeffs3,V3...,delta)[1],linear_coeffs3)
	@test isapprox(DiscretisedFluidQueues.linear(linear_coeffs3,V3...,delta)[2],0.0)
	delta = 0.25
	@test isapprox(DiscretisedFluidQueues.linear(linear_coeffs3,V3...,delta)[1],linear_coeffs3)
	@test isapprox(DiscretisedFluidQueues.linear(linear_coeffs3,V3...,delta)[2],0.0)

	linear_coeffs3 = [0.0; 2/3; 1/3]
	delta = 1.0
	@test isapprox(DiscretisedFluidQueues.linear(linear_coeffs3,V3...,delta)[1],linear_coeffs3)
	@test isapprox(DiscretisedFluidQueues.linear(linear_coeffs3,V3...,delta)[2],2.0/delta^2)
	delta = 0.25
	@test isapprox(DiscretisedFluidQueues.linear(linear_coeffs3,V3...,delta)[1],linear_coeffs3)
	@test isapprox(DiscretisedFluidQueues.linear(linear_coeffs3,V3...,delta)[2],2.0/delta^2)

	quadratic_coeffs3 = [0.0; 1.0; 0.0]
	linear_coeffs3 = [1/6; 2/3; 1/6]
	delta = 1.0
	@test isapprox(DiscretisedFluidQueues.linear(quadratic_coeffs3,V3...,delta)[1],linear_coeffs3)
	@test isapprox(DiscretisedFluidQueues.linear(quadratic_coeffs3,V3...,delta)[2],0.0)
	delta = 0.25
	@test isapprox(DiscretisedFluidQueues.linear(quadratic_coeffs3,V3...,delta)[1],linear_coeffs3)
	@test isapprox(DiscretisedFluidQueues.linear(quadratic_coeffs3,V3...,delta)[2],0.0)

	T_limiter = [0.0][:,:]
	c_limiter = [0.0]
	m_limiter = BoundedFluidQueue(T_limiter,c_limiter,2.0)
	nodes = collect(0.0:0.1:2.0)
	mesh_limiter = DGMesh(nodes,3)
	dq = DiscretisedFluidQueue(m_limiter,mesh_limiter)
	d0 = SFMDistribution((x,i)->x,dq)
	# limiting this linear function should do nothing
	# @test isapprox(limit(d0).coeffs,d0.coeffs; atol=1e-8)
	@test isapprox(limit(d0).coeffs[:],
		GeneralisedMUSCL.fun(d0.coeffs[:],GeneralisedMUSCL.generate_params(d0.dq)...);
		atol = 1e-8)
	@test d0.coeffs[:]==NoLimiter.fun(d0.coeffs[:],NoLimiter.generate_params(d0.dq)...)

	mesh_limiter = DGMesh(nodes,4)
	dq = DiscretisedFluidQueue(m_limiter,mesh_limiter)
	d0 = interior_point_mass(0.05,1,dq)
	# limiting this cubic approx to a point mass should do something
	@test !isapprox(limit(d0).coeffs,d0.coeffs; atol=1e-8)
	@test d0.coeffs[:]==NoLimiter.fun(d0.coeffs[:],NoLimiter.generate_params(d0.dq)...)
end


using PrimeSieve
using Base.Test

include("msieve_test.jl")
include("gaussian_test.jl")
include("tuples_test.jl")
include("primecount_test.jl")
include("primesieve_test.jl")
include("test_readme.jl")  # this is slow too.
include("slow_test.jl")


@testset "JuMP direct model - Linear Constraints - Affine parameters" begin
    optimizer = POI.Optimizer(GLPK.Optimizer())

    model = direct_model(optimizer)

    @variable(model, x[i = 1:2] >= 0)

    @variable(model, y in POI.Parameter(0))
    @variable(model, w in POI.Parameter(0))
    @variable(model, z in POI.Parameter(0))

    @constraint(model, 2 * x[1] + x[2] + y <= 4)
    @constraint(model, 1 * x[1] + 2 * x[2] + z <= 4)

    @objective(model, Max, 4 * x[1] + 3 * x[2] + w)

    optimize!(model)

    @test isapprox.(value(x[1]), 4.0 / 3.0, atol = ATOL)
    @test isapprox.(value(x[2]), 4.0 / 3.0, atol = ATOL)
    @test isapprox.(value(y), 0, atol = ATOL)

    # ===== Set parameter value =====
    MOI.set(model, POI.ParameterValue(), y, 2.0)
    optimize!(model)

    @test isapprox.(value(x[1]), 0.0, atol = ATOL)
    @test isapprox.(value(x[2]), 2.0, atol = ATOL)
    @test isapprox.(value(y), 2.0, atol = ATOL)
end

@testset "JuMP direct model - Linear Constraints - Parameter x variable" begin
    optimizer = POI.Optimizer(GLPK.Optimizer())

    model = direct_model(optimizer)

    @variable(model, x[i = 1:2] >= 0)

    @variable(model, y in POI.Parameter(0))
    @variable(model, w in POI.Parameter(0))
    @variable(model, z in POI.Parameter(0))

    @constraint(model, 2 * x[1] + x[2] + y <= 4)
    @constraint(model, (1 + y) * x[1] + 2 * x[2] + z <= 4)

    @objective(model, Max, 4 * x[1] + 3 * x[2] + w)

    optimize!(model)

    @test isapprox.(value(x[1]), 4.0 / 3.0, atol = ATOL)
    @test isapprox.(value(x[2]), 4.0 / 3.0, atol = ATOL)
    @test isapprox.(value(y), 0, atol = ATOL)

    # ===== Set parameter value =====
    MOI.set(model, POI.ParameterValue(), y, 2.0)
    optimize!(model)

    @test isapprox.(value(x[1]), 0.0, atol = ATOL)
    @test isapprox.(value(x[2]), 2.0, atol = ATOL)
    @test isapprox.(value(y), 2.0, atol = ATOL)
end

@testset "JuMP - Linear Constraints - Affine parameters" begin
    model = Model(() -> POI.Optimizer(GLPK.Optimizer()))

    @variable(model, x[i = 1:2] >= 0)

    @variable(model, y in POI.Parameter(0))
    @variable(model, w in POI.Parameter(0))
    @variable(model, z in POI.Parameter(0))

    @constraint(model, 2 * x[1] + x[2] + y <= 4)
    @constraint(model, 1 * x[1] + 2 * x[2] + z <= 4)

    @objective(model, Max, 4 * x[1] + 3 * x[2] + w)

    optimize!(model)

    @test isapprox.(value(x[1]), 4.0 / 3.0, atol = ATOL)
    @test isapprox.(value(x[2]), 4.0 / 3.0, atol = ATOL)
    @test isapprox.(value(y), 0, atol = ATOL)

    # ===== Set parameter value =====
    MOI.set(model, POI.ParameterValue(), y, 2.0)
    optimize!(model)

    @test isapprox.(value(x[1]), 0.0, atol = ATOL)
    @test isapprox.(value(x[2]), 2.0, atol = ATOL)
    @test isapprox.(value(y), 2.0, atol = ATOL)
end

@testset "JuMP - Linear Constraints - Parameter x variable" begin
    model = Model(() -> POI.Optimizer(GLPK.Optimizer()))

    @variable(model, x[i = 1:2] >= 0)

    @variable(model, y in POI.Parameter(0))
    @variable(model, w in POI.Parameter(0))
    @variable(model, z in POI.Parameter(0))

    @test MOI.get(model, POI.ParameterValue(), y) == 0

    @constraint(model, 2 * x[1] + x[2] + y <= 4)
    @constraint(model, (1 + y) * x[1] + 2 * x[2] + z <= 4)

    @objective(model, Max, 4 * x[1] + 3 * x[2] + w)

    optimize!(model)

    @test isapprox.(value(x[1]), 4.0 / 3.0, atol = ATOL)
    @test isapprox.(value(x[2]), 4.0 / 3.0, atol = ATOL)
    @test isapprox.(value(y), 0, atol = ATOL)

    # ===== Set parameter value =====
    MOI.set(model, POI.ParameterValue(), y, 2.0)
    optimize!(model)

    @test isapprox.(value(x[1]), 0.0, atol = ATOL)
    @test isapprox.(value(x[2]), 2.0, atol = ATOL)
    @test isapprox.(value(y), 2.0, atol = ATOL)
end

@testset "JuMP ConstraintFunction and ObjectiveFunction getters" begin
    model = direct_model(
        POI.Optimizer(
            MOI.Utilities.CachingOptimizer(
                MOI.Utilities.UniversalFallback(MOI.Utilities.Model{Float64}()),
                MOI.Utilities.AUTOMATIC,
            ),
        ),
    )

    vx = @variable(model, x[i = 1:2])
    vp = @variable(model, p[i = 1:2] in POI.Parameter.(-1))
    c1 = @constraint(model, con, sum(x) + sum(p) >= 1)
    c2 = @constraint(model, conq, sum(x .* p) >= 1)
    c3 = @constraint(model, conqa, sum(x .* p) + x[1]^2 + x[1] + p[1] >= 1)

    @test MOI.get(model, MOI.ConstraintFunction(), c1) ≈
          MOI.ScalarAffineFunction{Float64}(
        [
            MOI.ScalarAffineTerm{Float64}(1.0, MOI.VariableIndex(1)),
            MOI.ScalarAffineTerm{Float64}(1.0, MOI.VariableIndex(2)),
            MOI.ScalarAffineTerm{Float64}(
                1.0,
                MOI.VariableIndex(POI.PARAMETER_INDEX_THRESHOLD + 1),
            ),
            MOI.ScalarAffineTerm{Float64}(
                1.0,
                MOI.VariableIndex(POI.PARAMETER_INDEX_THRESHOLD + 2),
            ),
        ],
        0.0,
    )

    @test MOI.get(model, MOI.ConstraintFunction(), c2) ≈
          MOI.ScalarQuadraticFunction{Float64}(
        [
            MOI.ScalarQuadraticTerm{Float64}(
                1.0,
                MOI.VariableIndex(1),
                MOI.VariableIndex(POI.PARAMETER_INDEX_THRESHOLD + 1),
            ),
            MOI.ScalarQuadraticTerm{Float64}(
                1.0,
                MOI.VariableIndex(2),
                MOI.VariableIndex(POI.PARAMETER_INDEX_THRESHOLD + 2),
            ),
        ],
        [],
        0.0,
    )

    @test MOI.get(model, MOI.ConstraintFunction(), c3) ≈
          MOI.ScalarQuadraticFunction{Float64}(
        [
            MOI.ScalarQuadraticTerm{Float64}(
                1.0,
                MOI.VariableIndex(1),
                MOI.VariableIndex(POI.PARAMETER_INDEX_THRESHOLD + 1),
            ),
            MOI.ScalarQuadraticTerm{Float64}(
                1.0,
                MOI.VariableIndex(2),
                MOI.VariableIndex(POI.PARAMETER_INDEX_THRESHOLD + 2),
            ),
            MOI.ScalarQuadraticTerm{Float64}(
                2.0,
                MOI.VariableIndex(1),
                MOI.VariableIndex(1),
            ),
        ],
        [
            MOI.ScalarAffineTerm{Float64}(1.0, MOI.VariableIndex(1)),
            MOI.ScalarAffineTerm{Float64}(
                1.0,
                MOI.VariableIndex(POI.PARAMETER_INDEX_THRESHOLD + 1),
            ),
        ],
        0.0,
    )

    o1 = @objective(model, Min, sum(x) + sum(p))

    F = MOI.get(model, MOI.ObjectiveFunctionType())
    @test MOI.get(model, MOI.ObjectiveFunction{F}()) ≈
          MOI.ScalarAffineFunction{Float64}(
        [
            MOI.ScalarAffineTerm{Float64}(1.0, MOI.VariableIndex(1)),
            MOI.ScalarAffineTerm{Float64}(1.0, MOI.VariableIndex(2)),
            MOI.ScalarAffineTerm{Float64}(
                1.0,
                MOI.VariableIndex(POI.PARAMETER_INDEX_THRESHOLD + 1),
            ),
            MOI.ScalarAffineTerm{Float64}(
                1.0,
                MOI.VariableIndex(POI.PARAMETER_INDEX_THRESHOLD + 2),
            ),
        ],
        0.0,
    )

    o2 = @objective(model, Min, sum(x .* p) + 2)

    F = MOI.get(model, MOI.ObjectiveFunctionType())
    @test MOI.get(model, MOI.ObjectiveFunction{F}()) ≈
          MOI.ScalarQuadraticFunction{Float64}(
        [
            MOI.ScalarQuadraticTerm{Float64}(
                1.0,
                MOI.VariableIndex(1),
                MOI.VariableIndex(POI.PARAMETER_INDEX_THRESHOLD + 1),
            ),
            MOI.ScalarQuadraticTerm{Float64}(
                1.0,
                MOI.VariableIndex(2),
                MOI.VariableIndex(POI.PARAMETER_INDEX_THRESHOLD + 2),
            ),
        ],
        [],
        2.0,
    )

    o3 = @objective(model, Min, sum(x .* p) + x[1]^2 + x[1] + p[1])

    F = MOI.get(model, MOI.ObjectiveFunctionType())
    @test MOI.get(model, MOI.ObjectiveFunction{F}()) ≈
          MOI.ScalarQuadraticFunction{Float64}(
        [
            MOI.ScalarQuadraticTerm{Float64}(
                1.0,
                MOI.VariableIndex(1),
                MOI.VariableIndex(POI.PARAMETER_INDEX_THRESHOLD + 1),
            ),
            MOI.ScalarQuadraticTerm{Float64}(
                1.0,
                MOI.VariableIndex(2),
                MOI.VariableIndex(POI.PARAMETER_INDEX_THRESHOLD + 2),
            ),
            MOI.ScalarQuadraticTerm{Float64}(
                2.0,
                MOI.VariableIndex(1),
                MOI.VariableIndex(1),
            ),
        ],
        [
            MOI.ScalarAffineTerm{Float64}(1.0, MOI.VariableIndex(1)),
            MOI.ScalarAffineTerm{Float64}(
                1.0,
                MOI.VariableIndex(POI.PARAMETER_INDEX_THRESHOLD + 1),
            ),
        ],
        0.0,
    )
end

@testset "JuMP Interpret parametric bounds simple example" begin
    model = Model(() -> POI.Optimizer(GLPK.Optimizer()))
    MOI.set(model, POI.ConstraintsInterpretation(), POI.ONLY_BOUNDS)

    @variable(model, x[i = 1:2])
    @variable(model, p[i = 1:2] in POI.Parameter.(-1))
    @constraint(model, [i in 1:2], x[i] >= p[i])
    @objective(model, Min, sum(x))
    optimize!(model)

    @test MOI.get(model, MOI.ListOfConstraintTypesPresent()) ==
          Tuple{Type,Type}[
        (MOI.ScalarAffineFunction{Float64}, MOI.GreaterThan{Float64}),
        (MOI.VariableIndex, POI.Parameter),
    ]

    @test MOI.get(
        backend(model).optimizer.model.optimizer,
        MOI.ListOfConstraintTypesPresent(),
    ) == Tuple{Type,Type}[(MOI.VariableIndex, MOI.GreaterThan{Float64})]

    @test objective_value(model) == -2

    MOI.set(model, POI.ParameterValue(), p[1], 4.0)

    optimize!(model)
    @test objective_value(model) == 3
end

@testset "JuMP Interpret parametric bounds parametric expression" begin
    model = Model(() -> POI.Optimizer(GLPK.Optimizer()))
    MOI.set(model, POI.ConstraintsInterpretation(), POI.ONLY_BOUNDS)

    @variable(model, x[i = 1:2])
    @variable(model, p[i = 1:2] in POI.Parameter.(-1))
    @constraint(model, [i in 1:2], x[i] >= p[i] + p[1])
    @objective(model, Min, sum(x))
    optimize!(model)

    @test MOI.get(model, MOI.ListOfConstraintTypesPresent()) ==
          Tuple{Type,Type}[
        (MOI.ScalarAffineFunction{Float64}, MOI.GreaterThan{Float64}),
        (MOI.VariableIndex, POI.Parameter),
    ]

    @test MOI.get(
        backend(model).optimizer.model.optimizer,
        MOI.ListOfConstraintTypesPresent(),
    ) == Tuple{Type,Type}[(MOI.VariableIndex, MOI.GreaterThan{Float64})]

    @test objective_value(model) == -4

    MOI.set(model, POI.ParameterValue(), p[1], 4.0)

    optimize!(model)
    @test objective_value(model) == 11.0
end

@testset "JuMP Interpret parametric bounds direct model" begin
    model = direct_model(POI.Optimizer(GLPK.Optimizer()))
    MOI.set(model, POI.ConstraintsInterpretation(), POI.ONLY_BOUNDS)

    @variable(model, x[i = 1:2])
    @variable(model, p[i = 1:2] in POI.Parameter.(-1))
    @constraint(model, [i in 1:2], x[i] >= p[i])
    @objective(model, Min, sum(x))
    optimize!(model)

    @test MOI.get(model, MOI.ListOfConstraintTypesPresent()) ==
          Tuple{Type,Type}[(MOI.VariableIndex, MOI.GreaterThan{Float64})]

    @test MOI.get(
        backend(model).optimizer,
        MOI.ListOfConstraintTypesPresent(),
    ) == Tuple{Type,Type}[(MOI.VariableIndex, MOI.GreaterThan{Float64})]

    @test objective_value(model) == -2

    MOI.set(model, POI.ParameterValue(), p[1], 4.0)

    optimize!(model)
    @test objective_value(model) == 3
end

@testset "JuMP Interpret parametric bounds direct model no interpretation" begin
    model = direct_model(POI.Optimizer(GLPK.Optimizer()))
    MOI.set(model, POI.ConstraintsInterpretation(), POI.ONLY_CONSTRAINTS)

    @variable(model, x[i = 1:2])
    @variable(model, p[i = 1:2] in POI.Parameter.(-1))
    @constraint(model, [i in 1:2], x[i] >= p[i])
    @objective(model, Min, sum(x))
    optimize!(model)

    @test MOI.get(model, MOI.ListOfConstraintTypesPresent()) ==
          Tuple{Type,Type}[(
        MOI.ScalarAffineFunction{Float64},
        MOI.GreaterThan{Float64},
    )]

    @test MOI.get(
        backend(model).optimizer,
        MOI.ListOfConstraintTypesPresent(),
    ) == Tuple{Type,Type}[(
        MOI.ScalarAffineFunction{Float64},
        MOI.GreaterThan{Float64},
    )]

    @test objective_value(model) == -2

    MOI.set(model, POI.ParameterValue(), p[1], 4.0)

    optimize!(model)
    @test objective_value(model) == 3
end

@testset "JuMP Interpret parametric bounds direct model change interpretation" begin
    model = direct_model(POI.Optimizer(GLPK.Optimizer()))
    MOI.set(model, POI.ConstraintsInterpretation(), POI.ONLY_BOUNDS)
    @variable(model, x[i = 1:2])
    @variable(model, p[i = 1:2] in POI.Parameter.(-1))
    @constraint(model, [i in 1:2], x[i] >= p[i])
    @test_throws ErrorException @constraint(model, [i in 1:2], 2x[i] >= p[i])
    MOI.set(model, POI.ConstraintsInterpretation(), POI.ONLY_CONSTRAINTS)
    @constraint(model, [i in 1:2], 2x[i] >= p[i])
    @objective(model, Min, sum(x))
    optimize!(model)

    @test objective_value(model) == -1

    MOI.set(model, POI.ParameterValue(), p[1], 4.0)

    optimize!(model)
    @test objective_value(model) == 3.5
end

@testset "JuMP Interpret parametric bounds direct model both interpretations" begin
    model = direct_model(POI.Optimizer(GLPK.Optimizer()))
    MOI.set(model, POI.ConstraintsInterpretation(), POI.BOUNDS_AND_CONSTRAINTS)
    @variable(model, x[i = 1:2])
    @variable(model, p[i = 1:2] in POI.Parameter.(-1))
    @constraint(model, [i in 1:2], x[i] >= p[i])
    @constraint(model, [i in 1:2], 2x[i] >= p[i])
    @objective(model, Min, sum(x))
    optimize!(model)

    @test objective_value(model) == -1

    MOI.set(model, POI.ParameterValue(), p[1], 4.0)

    optimize!(model)
    @test objective_value(model) == 3.5
end

@testset "JuMP Interpret parametric bounds parametric not a valid bound" begin
    model = direct_model(POI.Optimizer(GLPK.Optimizer()))
    MOI.set(model, POI.ConstraintsInterpretation(), POI.ONLY_BOUNDS)

    @variable(model, x[i = 1:2])
    @variable(model, p[i = 1:2] in POI.Parameter.(-1))
    @test_throws ErrorException @constraint(
        model,
        [i in 1:2],
        2x[i] >= p[i] + p[1]
    )
end


"""
    convorder(alg::AbstractIteratedIntegralAlgorithm)

Returns the convergence order of the algorithm w.r.t. the truncation parameter.

See also: [`errcoeff`](@ref)
"""
function convorder end

"""
    errcoeff(dim, stepsize, alg, norm)
    errcoeff(dim, q_12, stepsize, alg, norm)

Returns the coefficient of the truncation parameter in the error estimate.
I.e. the error estimate is of the form

```math
\\lVert I(h)-\\tilde{I}^{(p)}(h) \\rVert_* ≤ \\mathrm{errcoeff}(m,h) \\cdot p^{-γ}
```

where the norm is given by `norm`, the approximation ``\\tilde{I}^{(p)}`` is
calculated using `alg` and ``γ`` is the order of convergence given by [`convorder(alg)`](@ref).
"""
function errcoeff end

"""
    terms_needed(dim, stepsize, eps, alg, norm)

Returns the number of terms in the approximating sum that is needed to ensure an error
in the given norm of at most `eps`.
This depends on the dimension of the Wiener process `dim`, the current stepsize and the chosen algorithm.

See also: [`AbstractIteratedIntegralAlgorithm`](@ref), [`AbstractErrorNorm`](@ref)

# Examples
```jldoctest; setup=:(using LevyArea)
julia> h = 1/128;

julia> terms_needed(10, h, h^(3/2), Milstein(), MaxL2())
7
```

# Implementation
New algorithms should only have to implement [`errcoeff`](@ref) and [`convorder`](@ref).
"""
function terms_needed(dim, stepsize, eps, alg::AbstractIteratedIntegralAlgorithm, norm::AbstractErrorNorm)
    ceil(Int64, (errcoeff(dim, stepsize, alg, norm)/eps)^(1//convorder(alg)) )
end

"""
    terms_needed(dim, q_12, stepsize, eps, alg, norm)

Used for finite-dimensional approximations of a Q-Wiener process with covariance matrix
``Q = Q^\\frac{1}{2}*Q^\\frac{1}{2}``. Here `q_12` is a vector of the eigenvalues of ``Q^\\frac{1}{2}``;
the square root of the covariance matrix. Equivalently these are the square roots of the eigenvalues of ``Q``.

# Examples
```jldoctest; setup=:(using LevyArea)
julia> h = 1/128;

julia> dim = 10;

julia> q = [1/k^2 for k=1:dim];

julia> terms_needed(dim, sqrt.(q), h, h^(3/2), Milstein(), FrobeniusL2())
9
```
"""
function terms_needed(dim, q_12, stepsize, eps, alg::AbstractIteratedIntegralAlgorithm, norm::AbstractErrorNorm)
    length(q_12) == dim || throw(ArgumentError("Length of q_12 must be equal to the dimension."))
    ceil(Int64, (errcoeff(dim, q_12, stepsize, alg, norm)/eps)^(1//convorder(alg)) )
end

"""
    norv(dim, n, alg::AbstractIteratedIntegralAlgorithm)

Returns the number of random numbers needed to simulate the iterated integrals
for a Wiener process of dimension `dim` and with truncation parameter `n`.
"""
norv(dim, n, alg::AbstractIteratedIntegralAlgorithm)

"""
    effective_cost(dim, stepsize, eps, alg, norm)
    effective_cost(dim, q_12, stepsize, eps, alg, norm)

Returns the number of random numbers needed to simulate the iterated integrals
with the given parameters.
"""
function effective_cost(dim, stepsize, eps, alg, norm)
    norv(dim, terms_needed(dim, stepsize, eps, alg, norm), alg)
end
function effective_cost(dim, q_12, stepsize, eps, alg, norm)
    norv(dim, terms_needed(dim, q_12, stepsize, eps, alg, norm), alg)
end

"""
    optimal_algorithm(dim, stepsize, eps=stepsize^(3/2), norm=MaxL2())
    optimal_algorithm(dim, q_12, stepsize, eps, norm=FrobeniusL2())

Returns the optimal algorithm for the given parameters,
i.e. the algorithm that needs to simulate the fewest random numbers
to achieve the desired precision `eps`.

# Examples
```jldoctest; setup=:(using LevyArea)
julia> h = 1/128;

julia> optimal_algorithm(10, h, h^(3/2), MaxL2())
MronRoe()

julia> optimal_algorithm(10, 1.0./(1:10).^2, h, h^(3/2), FrobeniusL2())
Milstein()
```
"""
function optimal_algorithm(dim, stepsize, eps=stepsize^(3/2), norm::AbstractErrorNorm=MaxL2())
    ind = argmin([effective_cost(dim, stepsize, eps, alg, norm) for alg ∈ ITER_INT_ALGS])
    return ITER_INT_ALGS[ind]
end
function optimal_algorithm(dim, q_12, stepsize, eps, norm::AbstractErrorNorm=FrobeniusL2())
    ind = argmin([effective_cost(dim, q_12, stepsize, eps, alg, norm) for alg ∈ ITER_INT_ALGS])
    return ITER_INT_ALGS[ind]
end