text:
function wipeConfigTable( mpSim::MPsim )

    DBInterface.execute( mpSim.simDB, "DROP TABLE IF EXISTS config" )
    sqliteCmd = string( "CREATE TABLE config(",
        "\n    parName TEXT,",
        "\n    parType TEXT,",
        "\n    parValue TEXT )" )
    DBInterface.execute( mpSim.simDB, sqliteCmd )

end  # wipeConfigTable( mpSim )

function storeGeneralPars( mpSim::MPsim )

    sqliteCmd = string( "INSERT INTO config (parName, parType, parValue)",
        " VALUES",
        "\n    ('Sim name', 'General', '", mpSim.simName, "'),",
        "\n    ('ID key', 'General', '", mpSim.idKey, "'),",
        "\n    ('Personnel target', 'General', '", mpSim.personnelTarget, "'),",
        "\n    ('Sim length', 'General', '", mpSim.simLength, "'),",
        "\n    ('Current time', 'General', '", now( mpSim ), "'),",
        "\n    ('DB commits', 'General', '", mpSim.nCommits, "')" )
    DBInterface.execute( mpSim.simDB, sqliteCmd )

end  # storeGeneralPars( mpSim )

function storeAttributes( mpSim::MPsim )

    if isempty( mpSim.attributeList )
        return
    end  # if isempty( mpSim.attributeList )

    sqliteCmd = map( collect( keys( mpSim.attributeList ) ) ) do name
        attribute = mpSim.attributeList[name]
        return string( "\n    ('", name, "', 'Attribute', '[",
            join( attribute.possibleValues, "," ), "];[",
            join( string.( attribute.initValues, ":",
            attribute.initValueWeights ), "," ), "]')" )
    end  # map( ... ) do name

    sqliteCmd = string( "INSERT INTO config (parName, parType, parValue)",
        " VALUES", join( sqliteCmd, "," ) )
    DBInterface.execute( mpSim.simDB, sqliteCmd )

end  # storeAttributres( mpSim )

function storeAttrition( mpSim )

    sqliteCmd = map( collect( keys( mpSim.attritionSchemes ) ) ) do name
        attrition = mpSim.attritionSchemes[name]
        return string( "\n    ('", name, "', 'Attrition', '", attrition.period,     ";[", join( string.( attrition.curvePoints, ":", attrition.rates ),
            "," ), "]')" )
    end  # map( ... ) do name

    sqliteCmd = string( "INSERT INTO config (parName, parType, parValue)",
        " VALUES", join( sqliteCmd, "," ) )
    DBInterface.execute( mpSim.simDB, sqliteCmd )

end  # storeAttrition( mpSim )

function storeNodes( mpSim::MPsim )

    baseNodeCmd = map( collect( keys( mpSim.baseNodeList ) ) ) do name
        node = mpSim.baseNodeList[name]
        tmpCmd = join( map( attr -> string( attr, ":",
            node.requirements[attr] ), collect( keys( node.requirements ) ) ),
            "," )
        return string( "\n    ('", name, "', 'Base Node', '", node.target, ";",
            node.attrition, ";[", tmpCmd, "]')" )
    end  # map( ... ) do name

    orderCmd = map( collect( keys( mpSim.baseNodeOrder ) ) ) do name
        return string( name, ":", mpSim.baseNodeOrder[name] )
    end  # map( ... ) do name

    orderCmd = string( "\n    ('Order', 'Base Node Order', '",
        join( orderCmd, ";" ), "')" )

    compNodeCmd = map( collect( keys( mpSim.compoundNodeList ) ) ) do name
        node = mpSim.compoundNodeList[name]
        return string( "\n    ('", name, "', 'Compound Node', '[",
            join( node.baseNodeList, "," ), "];", node.nodeTarget, "')" )
    end  # map( ... ) do name

    sqliteCmd = string( "INSERT INTO config (parName, parType, parValue)",
        " VALUES", join( baseNodeCmd, "," ), isempty( baseNodeCmd ) ? "" : ",",
        orderCmd, isempty( compNodeCmd ) ? "" : ",", join( compNodeCmd, "," ) )
    DBInterface.execute( mpSim.simDB, sqliteCmd )

end  # storeNodes( mpSim )

function storeRecruitment( mpSim::MPsim )

    if isempty( mpSim.recruitmentByName )
        return
    end  # if isempty( mpSim.recruitmentByName )

    sqliteCmd = map( collect( keys( mpSim.recruitmentByName ) ) ) do name
        recruitmentList = mpSim.recruitmentByName[name]

        return map( recruitmentList ) do recruitment
            # recNrConfig = nothing

            if recruitment.isAdaptive
                recNrConfig = string( recruitment.minRecruitment, ";",
                    recruitment.maxRecruitment )
            else
                recNrConfig = map( collect( keys(
                    recruitment.recruitmentDistNodes ) ) ) do amount
                    return string( amount, ":",
                        recruitment.recruitmentDistNodes[amount] )
                end  # map( ... ) do amount

                recNrConfig = string( recruitment.recruitmentDistType, ";[",
                    join( recNrConfig, "," ), "]" )
            end  # if recruitment.isAdaptive

            recAgeConfig = map( collect( keys(
                recruitment.ageDistNodes ) ) ) do age
                return string( age, ":", recruitment.ageDistNodes[age] )
            end  # map( ... ) do age

            recAgeConfig = string( recruitment.ageDistType, ";[",
                join( recAgeConfig, "," ), "]" )

            return string( "\n    ('", name, "', 'Recruitment', '",
                recruitment.freq, ";", recruitment.offset, ";",
                recruitment.targetNode, ";", recNrConfig, ";", recAgeConfig,
                "')" )
        end  # map( recruitmentList ) do recruitment
    end  # map( ... ) do name

    sqliteCmd = string( "INSERT INTO config (parName, parType, parValue)",
        " VALUES", join( join.( sqliteCmd, "," ), "," ) )
    DBInterface.execute( mpSim.simDB, sqliteCmd )

end  # storeRecruitment( mpSim )

function storeTransitions( mpSim )

    transCmd = map( collect( keys( mpSim.transitionsByName ) ) ) do name
        tmpCmd = map( mpSim.transitionsByName[name] ) do trans
            conditionsStr = map( trans.extraConditions ) do condition
                return string( condition.attribute, ":",
                    relationEntries[condition.operator], ":",
                    condition.value isa Vector ? join( condition.value, ":" ) :
                        condition.value )
            end  # map( trans.extraConditions ) do condition

            changesStr =
                map( collect( keys( trans.extraChanges ) ) ) do attribute
                    return string( attribute, ":",
                        trans.extraChanges[attribute] )
            end  # map( ... ) do attribute

            tmpStr = string( "'", trans.sourceNode, ";",
                trans.isOutTransition ? "OUT" : trans.targetNode, ";",
                trans.freq, ";", trans.offset, ";", trans.maxAttempts, ";",
                trans.minFlux, ";", trans.maxFlux, ";", trans.hasPriority,
                ";[", join( conditionsStr, "," ), "];[",
                join( changesStr, "," ), "];[", join( trans.probabilityList,
                "," ), "]'" )
            return tmpStr
        end  # map( ... ) do trans

        tmpCmd = join( string.( "\n    ('", name, "', 'Transition', ", tmpCmd,
            ")" ), "," )
    end  # map( ... ) do name

    orderCmd = map( collect( keys( mpSim.baseNodeOrder ) ) ) do name
        return string( name, ":", mpSim.baseNodeOrder[name] )
    end  # map( ... ) do name

    orderCmd = string( "\n    ('Order', 'Transition Order', '",
        join( orderCmd, ";" ), "')" )

    sqliteCmd = string( "INSERT INTO config (parName, parType, parValue)",
        " VALUES", join( transCmd, "," ), isempty( transCmd ) ? "" : ",",
        orderCmd )
    DBInterface.execute( mpSim.simDB, sqliteCmd )

end  # storeTransitions( mpSim )

function storeRetirement( mpSim )

    sqliteCmd = string( "INSERT INTO config (parName, parType, parValue)",
        " VALUES ('Retirement', 'Retirement', '", mpSim.retirement.freq, ";",
        mpSim.retirement.offset, ";", mpSim.retirement.maxCareerLength, ";",
        mpSim.retirement.retirementAge, ";", mpSim.retirement.isEither, "')" )
    DBInterface.execute( mpSim.simDB, sqliteCmd )

end  # storeRetirement( mpSim )


text:
using Test, Gnuplot
try
    @info "Gnuplot version: " * string(Gnuplot.gpversion())
catch
    Gnuplot.options.dry = true
end
push!(Gnuplot.options.init, "set term unknown")

x = [1, 2, 3]
y = [4, 5, 6]

s = Gnuplot.arrays2datablock(1)
@test all(s .== [" 1"])

s = Gnuplot.arrays2datablock(1, 2)
@test all(s .== [" 1 2"])

s = Gnuplot.arrays2datablock(x)
@test all(s .== [" 1"   ,
                 " 2"   ,
                 " 3"   ])

s = Gnuplot.arrays2datablock(x, y)
@test all(s .== [" 1 4",
                 " 2 5",
                 " 3 6"])

s = Gnuplot.arrays2datablock(x, y, x.+y)
@test all(s .== [" 1 4 5",
                 " 2 5 7",
                 " 3 6 9"])

z = [X+Y for X in x, Y in y];
s = Gnuplot.arrays2datablock(z)
@test all(s .== ["0 0 5",
                 "1 0 6",
                 "2 0 7",
                 ""     ,
                 "0 1 6",
                 "1 1 7",
                 "2 1 8",
                 ""     ,
                 "0 2 7",
                 "1 2 8",
                 "2 2 9"])

s = Gnuplot.arrays2datablock(z, z)
@test all(s .== [" 5 5",
                 " 6 6",
                 " 7 7",
                 ""    ,
                 " 6 6",
                 " 7 7",
                 " 8 8",
                 ""    ,
                 " 7 7",
                 " 8 8",
                 " 9 9"])

s = Gnuplot.arrays2datablock(x, y, z)
@test all(s .== [" 1 4 5" ,
                 " 2 4 6" ,
                 " 3 4 7" ,
                 ""       ,
                 " 1 5 6" ,
                 " 2 5 7" ,
                 " 3 5 8" ,
                 ""       ,
                 " 1 6 7" ,
                 " 2 6 8" ,
                 " 3 6 9" ])

c = [[X, Y] for Y in y for X in x];  # First Y (i.e. rows) then X (i.e. columns)
u = getindex.(c, 1)
v = getindex.(c, 2)

s = Gnuplot.arrays2datablock(u, v, z)
@test all(s .== [" 1 4 5" ,
                 " 2 4 6" ,
                 " 3 4 7" ,
                 ""       ,
                 " 1 5 6" ,
                 " 2 5 7" ,
                 " 3 5 8" ,
                 ""       ,
                 " 1 6 7" ,
                 " 2 6 8" ,
                 " 3 6 9" ])

s = Gnuplot.arrays2datablock(1:3, 1:3, ["One", "Two", "Three"])
@test all(s .== [ " 1 1 \"One\""  ,
                  " 2 2 \"Two\""  ,
                  " 3 3 \"Three\""])

#-----------------------------------------------------------------
pal = palette(:deepsea)
@test pal == "set palette defined (0.0 '#2B004D', 0.25 '#4E0F99', 0.5 '#3C54D4', 0.75 '#48A9F8', 1.0 '#C5ECFF')\nset palette maxcol 5\n"
ls = linetypes(:deepsea)
@test ls == "set linetype 1 lc rgb '#2B004D\nset linetype 2 lc rgb '#4E0F99\nset linetype 3 lc rgb '#3C54D4\nset linetype 4 lc rgb '#48A9F8\nset linetype 5 lc rgb '#C5ECFF\nset linetype cycle 5\n"

#-----------------------------------------------------------------
# Test wth empty dataset
@gp Float64[]
@gsp Float64[]
@gp Float64[] Float64[]
@gsp Float64[] Float64[]

#-----------------------------------------------------------------
x = collect(1.:100);

for i in 1:10
    @gp :gp1 "plot sin($i*x)"
    @gp :gp2 "plot sin($i*x)"
    @gp :gp3 "plot sin($i*x)"
    sleep(0.3)
end
Gnuplot.quitall()

#-----------------------------------------------------------------
@gp "plot sin(x)"
@gp "plot sin(x)" "pl cos(x)"
@gp "plo sin(x)" "s cos(x)"

@gp "plot sin(x)" :-
@gp :- "plot cos(x)"

@gp "plot sin(x)" 2 xr=(-2pi,2pi) "pause 2" "plot cos(4*x)"

x = range(-2pi, stop=2pi, length=100);
y = 1.5 * sin.(0.3 .+ 0.7x);
err = 0.1 * maximum(abs.(y)) .* fill(1, size(x));
noise = err .* randn(length(x));

h = hist(noise, nbins=10)
@gp h.bins h.counts "w histeps"

@gp x y
@gp x y "w l"

name = "\$MyDataSet1"
@gp name=>(x, y) "plot $name w l" "pl $name u 1:(2*\$2) w l"

@gsp randn(Float64, 30, 50)
@gp 1:30 1:50 randn(Float64, 30, 50) "w image"
@gsp x y y

@gp("set key horizontal", "set grid",
    xrange=(-7,7), ylabel="Y label",
    x, y, "w l t 'Real model' dt 2 lw 2 lc rgb 'red'",
    x, y+noise, err, "w errorbars t 'Data'")

@gp "f(x) = a * sin(b + c*x); a = 1; b = 1; c = 1;"   :-
@gp :- name=>(x, y+noise, err)                        :-
@gp :- "fit f(x) $name u 1:2:3 via a, b, c;"          :-
@gp :- "set multiplot layout 2,1"                     :-
@gp :- "plot $name w points" ylab="Data and model"    :-
@gp :- "plot $name u 1:(f(\$1)) w lines"              :-
@gp :- 2 xlab="X label" ylab="Residuals"              :-
@gp :- "plot $name u 1:((f(\$1)-\$2) / \$3):(1) w errorbars notit"

# Retrieve values for a, b and c
if Gnuplot.options.dry
    a = 1.5
    b = 0.3
    c = 0.7
else
    a = Meta.parse(gpexec("print a"))
    b = Meta.parse(gpexec("print b"))
    c = Meta.parse(gpexec("print c"))
end

@gp    :dry "f(x) = a * sin(b + c*x); a = 1; b = 1; c = 1;"  :-
@gp :- :dry "a = $a; b = $b; c = $c"                         :-
@gp :- :dry "set multiplot layout 2,1" ylab="Data and model" :-
name = "\$MyDataSet1"
@gp :- :dry name=>(x, y+noise, err)                          :-
@gp :- :dry "plot $name w points"                            :-
@gp :- :dry "plot $name u 1:(f(\$1)) w lines"                :-
@gp :- :dry 2 xlab="X label" ylab="Residuals"                :-
@gp :- :dry "plot $name u 1:((f(\$1)-\$2) / \$3):(1) w errorbars notit" :-
@gp :- :dry
save("test.gp")        # write on file test.gp
Gnuplot.quitall()
gpexec("load 'test.gp'") # load file test.gp

#-----------------------------------------------------------------
@gp("""
        approx_1(x) = x - x**3/6
        approx_2(x) = x - x**3/6 + x**5/120
        approx_3(x) = x - x**3/6 + x**5/120 - x**7/5040
        label1 = "x - {x^3}/3!"
        label2 = "x - {x^3}/3! + {x^5}/5!"
        label3 = "x - {x^3}/3! + {x^5}/5! - {x^7}/7!"
        #
        set termoption enhanced
        save_encoding = GPVAL_ENCODING
        set encoding utf8
        #
        set title "Polynomial approximation of sin(x)"
        set key Left center top reverse
        set xrange [ -3.2 : 3.2 ]
        set xtics ("-π" -pi, "-π/2" -pi/2, 0, "π/2" pi/2, "π" pi)
        set format y "%.1f"
        set samples 500
        set style fill solid 0.4 noborder""",
    "plot '+' using 1:(sin(\$1)):(approx_1(\$1)) with filledcurve title label1 lt 3",
    "plot '+' using 1:(sin(\$1)):(approx_2(\$1)) with filledcurve title label2 lt 2",
    "plot '+' using 1:(sin(\$1)):(approx_3(\$1)) with filledcurve title label3 lt 1",
    "plot sin(x) with lines lw 1 lc rgb 'black'")

#-----------------------------------------------------------------
@gp("""
        set zrange [-1:1]
        unset label
        unset arrow
        sinc(u,v) = sin(sqrt(u**2+v**2)) / sqrt(u**2+v**2)
        set xrange [-5:5]; set yrange [-5:5]
        set arrow from 5,-5,-1.2 to 5,5,-1.2 lt -1
        set label 1 "increasing v" at 6,0,-1
        set arrow from 5,6,-1 to 5,5,-1 lt -1
        set label 2 "u=0" at 5,6.5,-1
        set arrow from 5,6,sinc(5,5) to 5,5,sinc(5,5) lt -1
        set label 3 "u=1" at 5,6.5,sinc(5,5)
        set parametric
        set hidden3d offset 0	# front/back coloring makes no sense for fenceplot #
        set isosamples 2,33
        xx=-5; dx=(4.99-(-4.99))/9
        x0=xx; xx=xx+dx
        x1=xx; xx=xx+dx
        x2=xx; xx=xx+dx
        x3=xx; xx=xx+dx
        x4=xx; xx=xx+dx
        x5=xx; xx=xx+dx
        x6=xx; xx=xx+dx
        x7=xx; xx=xx+dx
        x8=xx; xx=xx+dx
        x9=xx; xx=xx+dx""",
    "splot [u=0:1][v=-4.99:4.99]x0, v, (u<0.5) ? -1 : sinc(x0,v) notitle",
	"splot x1, v, (u<0.5) ? -1 : sinc(x1,v) notitle",
	"splot x2, v, (u<0.5) ? -1 : sinc(x2,v) notitle",
	"splot x3, v, (u<0.5) ? -1 : sinc(x3,v) notitle",
	"splot x4, v, (u<0.5) ? -1 : sinc(x4,v) notitle",
	"splot x5, v, (u<0.5) ? -1 : sinc(x5,v) notitle",
	"splot x6, v, (u<0.5) ? -1 : sinc(x6,v) notitle",
	"splot x7, v, (u<0.5) ? -1 : sinc(x7,v) notitle",
	"splot x8, v, (u<0.5) ? -1 : sinc(x8,v) notitle",
	"splot x9, v, (u<0.5) ? -1 : sinc(x9,v) notitle")
Gnuplot.quitall()

ext: jl


text:
## Double Pendulum
import TrajectoryOptimization
traj_folder = joinpath(dirname(pathof(TrajectoryOptimization)),"..")
urdf_folder = joinpath(traj_folder, "dynamics/urdf")
urdf_doublependulum = joinpath(urdf_folder, "doublependulum.urdf")

doublependulum = Model(urdf_doublependulum)

max_issues_repo_name: MehrdadZareian/GMARA

max_forks_repo_name: MehrdadZareian/GMARA


text:
const MOI = MathOptInterface
const CI = MOI.ConstraintIndex
const VI = MOI.VariableIndex

const MOIU = MOI.Utilities

Base.@kwdef mutable struct MOISolution
    ret_val::Int = 0
    raw_status::String = "Problem not solved"
    primal::Vector{Float64} = Float64[] # primal of variables
    dual_eq::Vector{Float64} = Float64[] # dual of constraints
    dual_in::Vector{Float64} = Float64[] # dual of constraints
    dual_cone::Vector{Float64} = Float64[] # dual of constraints
    slack_eq::Vector{Float64} = Float64[]
    slack_in::Vector{Float64} = Float64[]
    # slack_sd::Vector{Vector{Float64}}
    # slack_so::Vector{Vector{Float64}}
    primal_residual::Float64 = NaN
    dual_residual::Float64 = NaN
    objval::Float64 = NaN
    dual_objval::Float64 = NaN
    # obj_cte::Float64 = NaN # used in rays - see SCS
    gap::Float64 = NaN
    time::Float64 = NaN
    final_rank::Int = -1
    primal_feasible_user_tol::Bool = false
    dual_feasible_user_tol::Bool = false
    certificate_found::Bool = false
end

# `ModelData` struct is discarded
mutable struct ModelData
    m::Int # Number of rows/constraints
    n::Int # Number of cols/variables

    # equality matrix
    I::Vector{Int} # List of rows
    J::Vector{Int} # List of cols
    V::Vector{Float64} # List of coefficients
    # equality rhs
    b::Vector{Float64} # constants

    # inequality matrix
    Ii::Vector{Int} # List of rows
    Ji::Vector{Int} # List of cols
    Vi::Vector{Float64} # List of coefficients
    # inequality rhs
    h::Vector{Float64} # constants

    objective_constant::Float64 # The objective is min c'x + objective_constant
    c::Vector{Float64}
end

Base.@kwdef mutable struct ConeData

    cone_cols::Int = 0

    # matrix lines
    eq_tot::Int = 0 # number of linear equality constraints
    eq_start::Vector{Int} = Int[]
    eq_len::Vector{Int} = Int[]
    in_tot::Int = 0 # length of LP cone
    in_start::Vector{Int} = Int[]
    in_len::Vector{Int} = Int[]

    # cones
    sdc::Vector{Vector{Int}} = Vector{Int}[] # semidefinite
    soc::Vector{Vector{Int}} = Vector{Int}[] # second order
end

mutable struct Optimizer <: MOI.AbstractOptimizer
    cone::ConeData
    maxsense::Bool
    data::Union{Nothing, ModelData} # only non-Void between MOI.copy_to and MOI.optimize!
    sol::MOISolution
    options::Options
end
function Optimizer(;kwargs...)
    optimizer = Optimizer(ConeData(), false, nothing, MOISolution(), Options())
    for (key, value) in kwargs
        MOI.set(optimizer, MOI.RawParameter(key), value)
    end
    return optimizer
end

MOI.get(::Optimizer, ::MOI.SolverName) = "ProxSDP"

function MOI.set(optimizer::Optimizer, param::MOI.RawParameter, value)
    fields = fieldnames(Options)
    name = Symbol(param.name)
    if name in fields
        setfield!(optimizer.options, name, value)
    else
        error("No parameter matching $(name)")
    end
    return value
end
function MOI.get(optimizer::Optimizer, param::MOI.RawParameter)
    fields = fieldnames(Options)
    name = Symbol(param.name)
    if name in fields
        return getfield(optimizer.options, name)
    else
        error("No parameter matching $(name)")
    end
end

MOI.supports(::Optimizer, ::MOI.Silent) = true
function MOI.set(optimizer::Optimizer, ::MOI.Silent, value::Bool)
    if value == true
        optimizer.options.timer_verbose = false
    end
    optimizer.options.log_verbose = !value
end
function MOI.get(optimizer::Optimizer, ::MOI.Silent)
    if optimizer.options.log_verbose
        return false
    elseif optimizer.options.timer_verbose
        return false
    else
        return true
    end
end

MOI.supports(::Optimizer, ::MOI.TimeLimitSec) = true
function MOI.set(optimizer::Optimizer, ::MOI.TimeLimitSec, value)
    optimizer.options.time_limit = value
end
function MOI.get(optimizer::Optimizer, ::MOI.TimeLimitSec)
    return optimizer.options.time_limit
end

MOI.supports(::Optimizer, ::MOI.NumberOfThreads) = false

function MOI.is_empty(optimizer::Optimizer)
    !optimizer.maxsense && optimizer.data === nothing
end
function MOI.empty!(optimizer::Optimizer)
    optimizer.maxsense = false
    optimizer.data = nothing # It should already be nothing except if an error is thrown inside copy_to
    optimizer.cone = ConeData()
    optimizer.sol.ret_val = 0
end

MOIU.supports_allocate_load(::Optimizer, copy_names::Bool) = !copy_names

function MOI.supports(::Optimizer,
                      ::Union{MOI.ObjectiveSense,
                              MOI.ObjectiveFunction{MOI.SingleVariable},
                              MOI.ObjectiveFunction{MOI.ScalarAffineFunction{Float64}}})
    return true
end

function MOI.supports_constraint(
    ::Optimizer,
    ::Type{<:MOI.VectorAffineFunction{Float64}},
    ::Type{<:Union{MOI.Zeros, MOI.Nonpositives}})
    return true
end

function MOI.supports_constraint(
    ::Optimizer,
    ::Type{<:MOI.VectorOfVariables},
    ::Type{<:Union{MOI.SecondOrderCone,
                   MOI.PositiveSemidefiniteConeTriangle}})
    return true
end

function MOI.copy_to(dest::Optimizer, src::MOI.ModelLike; kws...)
    return MOIU.automatic_copy_to(dest, src; kws...)
end

const SupportedSets = Union{MOI.PositiveSemidefiniteConeTriangle,
                            MOI.SecondOrderCone}
function get_new_variable_indexes_unique(optimizer::Optimizer, set::SupportedSets)
    len = MOI.dimension(set)
    first = optimizer.cone.cone_cols + 1
    last = optimizer.cone.cone_cols + len
    optimizer.cone.cone_cols += len
    return collect(first:last)
end
function get_cone_list(cone::ConeData, set::MOI.PositiveSemidefiniteConeTriangle)
    return cone.sdc
end
function get_cone_list(cone::ConeData, set::MOI.SecondOrderCone)
    return cone.soc
end
function get_cone_list(optimizer::Optimizer, set::SupportedSets)
    get_cone_list(optimizer.cone, set)
end

function MOIU.allocate_constrained_variables(optimizer::Optimizer, set::SupportedSets)
    inds = get_new_variable_indexes_unique(optimizer,
    # func,
    set)
    vars = MOI.VariableIndex.(inds)
    list = get_cone_list(optimizer, set)
    push!(list, inds)
    ci = MOI.ConstraintIndex{MOI.VectorOfVariables, typeof(set)}(length(list))
    return vars, ci
end

function MOIU.load_constrained_variables(
    optimizer::Optimizer, vis::Vector{MOI.VariableIndex},
    ci::MOI.ConstraintIndex{MOI.VectorOfVariables},
    set::SupportedSets)
end

function _allocate_constraint(cone::ConeData, f, set::SupportedSets)
    list = get_cone_list(cone, set)
    push!(list, Int[])
    return length(list)
end

function _allocate_constraint(cone::ConeData, f, s::MOI.Zeros)
    len = MOI.dimension(s)
    push!(cone.eq_start, cone.eq_tot + 1)
    push!(cone.eq_len, len)
    cone.eq_tot += len
    return length(cone.eq_len)
end

function _allocate_constraint(cone::ConeData, f, s::MOI.Nonpositives)
    len = MOI.dimension(s)
    push!(cone.in_start, cone.in_tot + 1)
    push!(cone.in_len, len)
    cone.in_tot += len
    return length(cone.in_len)
end

function MOIU.load_constraint(optimizer::Optimizer, ci::CI,
    f::MOI.VectorOfVariables,
    set::SupportedSets)

    vec = get_cone_list(optimizer.cone, set)[ci.value]

    append!(vec, [i.value for i in f.variables])
    nothing
end

function MOIU.allocate_constraint(optimizer::Optimizer, f::F, s::S) where {F <: MOI.AbstractFunction, S <: MOI.AbstractSet}
    return CI{F, S}(_allocate_constraint(optimizer.cone, f, s))
end

# Vectorized length for matrix dimension n
sympackedlen(n) = div(n*(n+1), 2)
# Matrix dimension for vectorized length n
sympackeddim(n) = div(isqrt(1+8n) - 1, 2)

output_index(t::MOI.VectorAffineTerm) = t.output_index
variable_index_value(v::MOI.VariableIndex) = v.value
variable_index_value(t::MOI.ScalarAffineTerm) = t.variable_index.value
variable_index_value(t::MOI.VectorAffineTerm) = variable_index_value(t.scalar_term)
coefficient(t::MOI.ScalarAffineTerm) = t.coefficient
coefficient(t::MOI.VectorAffineTerm) = coefficient(t.scalar_term)

function MOIU.load_constraint(optimizer::Optimizer, ci::CI,
                              f::MOI.VectorAffineFunction,
                              set::MOI.AbstractVectorSet)

    n = length(f.terms)
    if n != 1
        A = sparse(output_index.(f.terms), variable_index_value.(f.terms),
        coefficient.(f.terms))
        # sparse combines duplicates with + but does
        # not remove zeros created so we call dropzeros!
        dropzeros!(A)
        I, J, V = findnz(A)
    else
        I, J, V = output_index.(f.terms), variable_index_value.(f.terms), coefficient.(f.terms)
    end
#=
    n = length(f.terms)
    I = Int[]
    J = Int[]
    V = Float64[]
    sizehint!(I, n)
    sizehint!(J, n)
    sizehint!(V, n)
    for i in 1:n
        if !iszero(coefficient(f.terms[i]))
            push!(I, output_index(f.terms[i]))
            push!(J, variable_index_value(f.terms[i]))
            push!(V, coefficient(f.terms[i]))
        end
    end
=#
    #=
    I,J,V = output_index.(f.terms), variable_index_value.(f.terms), coefficient.(f.terms)
    =#

    offset = ctr_offset(optimizer, set, ci)
    rows = 1:len(optimizer, set, ci)
    i = (offset-1) .+ rows

    rhs = matrix_rhs_vec(optimizer, set)
    rhs[i] .= -f.constants

    # MOI:  Ax + b {==, <=} 0
    # psdp: Ax {==, <=} -b
    I_, J_, V_ = matrix_triplets(optimizer, set)
    append!(I_, offset-1 .+ I)
    append!(J_, J)
    append!(V_, V)

    nothing
end
function ctr_offset(optimizer::Optimizer, set::MOI.Zeros, ci)
    return optimizer.cone.eq_start[ci.value]
end
function len(optimizer::Optimizer, set::MOI.Zeros, ci)
    return optimizer.cone.eq_len[ci.value]
end
function ctr_offset(optimizer::Optimizer, set::MOI.Nonpositives, ci)
    return optimizer.cone.in_start[ci.value]
end
function len(optimizer::Optimizer, set::MOI.Nonpositives, ci)
    return optimizer.cone.in_len[ci.value]
end
function matrix_triplets(optimizer::Optimizer, set::MOI.Zeros)
    return optimizer.data.I, optimizer.data.J, optimizer.data.V
end
function matrix_rhs_vec(optimizer::Optimizer, set::MOI.Zeros)
    return optimizer.data.b
end
function matrix_triplets(optimizer::Optimizer, set::MOI.Nonpositives)
    return optimizer.data.Ii, optimizer.data.Ji, optimizer.data.Vi
end
function matrix_rhs_vec(optimizer::Optimizer, set::MOI.Nonpositives)
    return optimizer.data.h
end

# called after ctr var - only aditonal
function MOIU.allocate_variables(optimizer::Optimizer, nvars::Integer)
    allocated = optimizer.cone.cone_cols
    VI.(allocated .+ (1:nvars))
end

function MOIU.load_variables(optimizer::Optimizer, nvars::Integer)
    # @show "load var"
    # @show nvars
    cone = optimizer.cone
    m = cone.eq_tot + cone.in_tot # + cone.q + cone.s + 3cone.ep + cone.ed
    I = Int[]
    J = Int[]
    V = Float64[]
    sizehint!(I, cone.eq_tot)
    sizehint!(J, cone.eq_tot)
    sizehint!(V, cone.eq_tot)
    b = zeros(cone.eq_tot)
    Ii = Int[]
    Ji = Int[]
    Vi = Float64[]
    sizehint!(Ii, cone.in_tot)
    sizehint!(Ji, cone.in_tot)
    sizehint!(Vi, cone.in_tot)
    h = zeros(cone.in_tot)

    tot_vars = nvars# + cone.cone_cols
    c = zeros(tot_vars)
    optimizer.data = ModelData(m, tot_vars, I, J, V, b, Ii, Ji, Vi, h, 0., c)
    # `optimizer.sol` contains the result of the previous optimization.
    # It is used as a warm start if its length is the same, e.g.
    # probably because no variable and/or constraint has been added.
    if length(optimizer.sol.primal) != tot_vars
        optimizer.sol.primal = zeros(tot_vars)
    end
    # TODO @joaquim
    # @assert length(optimizer.sol.dual) == length(optimizer.sol.slack)
    # if length(optimizer.sol.dual) != m
    #     optimizer.sol.dual = zeros(m)
    #     optimizer.sol.slack = zeros(m)
    # end
    return nothing
end

function MOIU.allocate(::Optimizer, ::MOI.VariablePrimalStart,
                       ::MOI.VariableIndex, ::Union{Nothing, Float64})
end
function MOIU.allocate(::Optimizer, ::MOI.ConstraintPrimalStart,
                       ::MOI.ConstraintIndex, ::Float64)
end
function MOIU.allocate(::Optimizer, ::MOI.ConstraintDualStart,
                       ::MOI.ConstraintIndex, ::Float64)
end
function MOIU.allocate(optimizer::Optimizer, ::MOI.ObjectiveSense,
                       sense::MOI.OptimizationSense)
    optimizer.maxsense = sense == MOI.MAX_SENSE
end
function MOIU.allocate(::Optimizer, ::MOI.ObjectiveFunction,
    ::MOI.Union{MOI.SingleVariable,
                MOI.ScalarAffineFunction{Float64}})
end

function MOIU.load(::Optimizer, ::MOI.VariablePrimalStart,
    ::MOI.VariableIndex, ::Nothing)
end
function MOIU.load(optimizer::Optimizer, ::MOI.VariablePrimalStart,
                   vi::MOI.VariableIndex, value::Float64)
    optimizer.sol.primal[vi.value] = value
end
function MOIU.load(::Optimizer, ::MOI.ConstraintPrimalStart,
    ::MOI.ConstraintIndex, ::Nothing)
end
function MOIU.load(::Optimizer, ::MOI.ConstraintDualStart,
    ::MOI.ConstraintIndex, ::Nothing)
end
function MOIU.load(::Optimizer, ::MOI.ObjectiveSense, ::MOI.OptimizationSense)
end
function MOIU.load(optimizer::Optimizer, ::MOI.ObjectiveFunction,
                   f::MOI.SingleVariable)
    MOIU.load(optimizer,
          MOI.ObjectiveFunction{MOI.ScalarAffineFunction{Float64}}(),
          MOI.ScalarAffineFunction{Float64}(f))
    return nothing
end
function MOIU.load(optimizer::Optimizer, ::MOI.ObjectiveFunction,
               f::MOI.ScalarAffineFunction)
    c0 = Vector(sparsevec(variable_index_value.(f.terms), coefficient.(f.terms),
                        optimizer.data.n))
    optimizer.data.objective_constant = f.constant
    optimizer.data.c = optimizer.maxsense ? -c0 : c0
    return nothing
end

matindices(n::Integer) = (LinearIndices(tril(trues(n,n))))[findall(tril(trues(n,n)))]

function MOI.optimize!(optimizer::Optimizer)
    if optimizer.data === nothing
        # optimize! has already been called and no new model has been copied
        return
    end

    # @show "in opt"

    TimerOutputs.reset_timer!()

    @timeit "pre-processing" begin

    # parse options
    options = optimizer.options

    cone = optimizer.cone

    #=
        Build linear sets
    =#

    m = optimizer.data.m #rows
    n = optimizer.data.n #cols

    objective_constant = optimizer.data.objective_constant
    # c = optimizer.data.c

    # Build Prox SDP Affine Sets
    # b = optimizer.data.b
    # A = sparse(optimizer.data.I, optimizer.data.J, optimizer.data.V, length(b), n)
    # h = optimizer.data.h
    # G = sparse(optimizer.data.Ii, optimizer.data.Ji, optimizer.data.Vi, length(h), n)

    # Dimensions (of affine sets)

    aff = AffineSets(0, 0, 0, 0,
    sparse(optimizer.data.I, optimizer.data.J, optimizer.data.V, length(optimizer.data.b), n),
    sparse(optimizer.data.Ii, optimizer.data.Ji, optimizer.data.Vi, length(optimizer.data.h), n),
    optimizer.data.b,
    optimizer.data.h,
    optimizer.data.c)
    # A, G, b, h, c)

    n_variables = size(aff.A)[2] # primal
    n_eqs = size(aff.A)[1]
    n_ineqs = size(aff.G)[1]
    aff.n = n_variables
    aff.p = n_eqs
    aff.m = n_ineqs

    #=
        Build conic sets
    =#

    # Build SDP Sets
    con = ConicSets(
        SDPSet[],
        SOCSet[]
        )

    # create extra variables
    n_tot_variables = n_variables
    In, Jn, Vn = Int[], Int[], Float64[]

    # this way there is a single elements per column
    # because we assume VOV in SET and NOT AFF in SET
    for i in eachindex(cone.soc)
        n_vars = length(cone.soc[i])
        vec_inds = cone.soc[i]
        n_tot_variables += fix_duplicates!(vec_inds, n_tot_variables, In, Jn, Vn)
        push!(con.socone, SOCSet(vec_inds, n_vars))
        # first_ind_local += n_vars
    end

    for i in eachindex(cone.sdc)
        n_vars = length(cone.sdc[i])
        vec_inds = cone.sdc[i]
        n_tot_variables += fix_duplicates!(vec_inds, n_tot_variables, In, Jn, Vn)
        sq_side = sympackeddim(n_vars)
        sq_len = sq_side*sq_side
        tri_len = n_vars
        mat_inds = matindices(sq_side)
        push!(con.sdpcone, SDPSet(vec_inds, mat_inds, tri_len, sq_len, sq_side))
        # first_ind_local += n_vars
    end

    optimizer.data = nothing # Allows GC to free optimizer.data before A is loaded

    #=
        Pre-process to remove duplicates from differente cones
    =#

    for i in 1:length(con.socone)
        for j in i+1:length(con.socone)
            vec1 = con.socone[i].idx
            vec2 = con.socone[j].idx
            n_tot_variables += fix_duplicates!(vec1, vec2, n_tot_variables, In, Jn, Vn)
        end
    end

    for i in 1:length(con.sdpcone)
        for j in i+1:length(con.sdpcone)
            vec1 = con.sdpcone[i].vec_i
            vec2 = con.sdpcone[j].vec_i
            n_tot_variables += fix_duplicates!(vec1, vec2, n_tot_variables, In, Jn, Vn)
        end
    end

    for i in 1:length(con.sdpcone)
        for j in 1:length(con.socone)
            vec1 = con.sdpcone[i].vec_i
            vec2 = con.socone[j].idx
            n_tot_variables += fix_duplicates!(vec1, vec2, n_tot_variables, In, Jn, Vn)
        end
    end

    n_new_variables = n_tot_variables - n_variables

    A2 = sparse(In .- n_variables, Jn, Vn, n_new_variables, n_tot_variables)
    b2 = zeros(n_new_variables)

    append!(aff.b, b2)
    aff.A = hvcat((2,1), aff.A, spzeros(size(aff.A)[1], n_new_variables), A2)
    aff.G = hcat(aff.G, spzeros(size(aff.G)[1], n_new_variables))
    append!(aff.c, zeros(n_new_variables))
    aff.n += n_new_variables
    aff.p += n_new_variables
    aff.extra = n_new_variables

    In, Jn, Vn = Int[], Int[], Float64[]
    A2 = sparse(In, Jn, Vn, 0, 0)

    #=
        Solve modified problem
    =#

    end

    # warm = WarmStart()

    if options.disable_julia_logger
        # disable logger
        global_log = Logging.current_logger()
        Logging.global_logger(Logging.NullLogger())
    end

    sol = @timeit "Main" chambolle_pock(aff, con, options)

    if options.disable_julia_logger
        # re-enable logger
        Logging.global_logger(global_log)
    end

    #=
        Unload solution
    =#

    ret_val = sol.status
    primal = sol.primal[1:aff.n-aff.extra]
    dual_cone = sol.dual_cone[1:aff.n-aff.extra]
    dual_eq = sol.dual_eq[1:aff.p-aff.extra]
    slack_eq = sol.slack_eq[1:aff.p-aff.extra]
    objval = sol.objval

    if options.timer_verbose
        TimerOutputs.print_timer(TimerOutputs.DEFAULT_TIMER)
        print("\n")
        TimerOutputs.print_timer(TimerOutputs.flatten(TimerOutputs.DEFAULT_TIMER))
        print("\n")
    end
    if options.timer_file
        f = open("time.log","w")
        TimerOutputs.print_timer(f,TimerOutputs.DEFAULT_TIMER)
        print(f,"\n")
        TimerOutputs.print_timer(f,TimerOutputs.flatten(TimerOutputs.DEFAULT_TIMER))
        print(f,"\n")
        close(f)
    end

    optimizer.sol = MOISolution(
        ret_val,
        sol.status_string,
        primal,
        dual_eq,
        sol.dual_in,
        dual_cone,
        slack_eq,
        sol.slack_in,
        sol.primal_residual,
        sol.dual_residual,
        (optimizer.maxsense ? -1 : 1) * objval+objective_constant,
        (optimizer.maxsense ? -1 : 1) * sol.dual_objval+objective_constant,
        sol.gap,
        sol.time,
        sol.final_rank,
        sol.primal_feasible_user_tol,
        sol.dual_feasible_user_tol,
        sol.certificate_found,
        )
end

function get_indices_cone(A, rows, n_vars, first_ind_local)
    vec_inds = zeros(Int, n_vars)
    for (idx, col) in enumerate(first_ind_local:first_ind_local+n_vars-1)
        # columns -> pos in cone (because is tranposed)
        for j in nzrange(A, col)
            row = rows[j]
            position = idx
            var_idx = row # global
            vec_inds[position] = var_idx
        end
    end
    return vec_inds
end

function fix_duplicates!(vec1::Vector{Int}, vec2::Vector{Int}, n::Int,
                         In::Vector{Int}, Jn::Vector{Int}, Vn::Vector{Float64})
    duplicates = intersect(vec2, vec1)
    n_dups = length(duplicates)
    if n_dups == 0
        return 0
    end

    append!(Jn, duplicates)
    append!(Jn, collect(1:n_dups) .+ n)
    append!(In, collect(1:n_dups) .+ n)
    append!(In, collect(1:n_dups) .+ n)
    append!(Vn,  ones(n_dups))
    append!(Vn, -ones(n_dups))
    cont = 1
    for i in eachindex(vec2)
        if vec2[i] == duplicates[cont]
            vec2[i] = n + cont
            cont += 1
        end
    end
    @assert cont-1 == n_dups
    return n_dups
end

function fix_duplicates!(vec::Vector{Int}, n::Int,
                         In::Vector{Int}, Jn::Vector{Int}, Vn::Vector{Float64})
    seen = Set{eltype(vec)}()
    dups = Vector{eltype(vec)}()
    new_vars = 0
    for i in eachindex(vec)
        x = vec[i]
        if in(x, seen)
            push!(dups, x)
            new_vars += 1
            vec[i] = new_vars + n
        else
            push!(seen, x)
        end
    end
    n_dups = length(dups)
    if n_dups == 0
        return 0
    end
    append!(Jn, dups)
    append!(Jn, collect(1:n_dups) .+ n)
    append!(In, collect(1:n_dups) .+ n)
    append!(In, collect(1:n_dups) .+ n)
    append!(Vn,  ones(n_dups))
    append!(Vn, -ones(n_dups))
    return n_dups
end

function ivech!(out::AbstractMatrix{T}, v::AbstractVector{T}) where T
    n = sympackeddim(length(v))
    n1, n2 = size(out)
    @assert n == n1 == n2
    c = 0
    for j in 1:n, i in 1:j
        c += 1
        out[i,j] = v[c]
    end
    return out
end
function ivech(v::AbstractVector{T}) where T
    n = sympackeddim(length(v))
    out = zeros(n, n)
    ivech!(out, v)

    return out
end

ivec(X) = Matrix(Symmetric(ivech(X),:U))

#=
    Status
=#

function MOI.get(optimizer::Optimizer, ::MOI.SolveTime)
    return optimizer.sol.time
end

function MOI.get(optimizer::Optimizer, ::MOI.RawStatusString)
    return optimizer.sol.raw_status
end

# Implements getter for result value and statuses
# ProxSDP returns one of the following integers:
# 0 - MOI.OPTIMIZE_NOT_CALLED
# 1 - MOI.OPTIMAL
# 2 - MOI.TIME_LIMIT
# 3 - MOI.ITERATION_LIMIT
# 4 - MOI.INFEASIBLE_OR_UNBOUNDED
function MOI.get(optimizer::Optimizer, ::MOI.TerminationStatus)
    s = optimizer.sol.ret_val
    @assert 0 <= s <= 6
    if s == 0
        return MOI.OPTIMIZE_NOT_CALLED
    elseif s == 1
        return MOI.OPTIMAL
    elseif s == 2
        return MOI.TIME_LIMIT
    elseif s == 3
        return MOI.ITERATION_LIMIT
    elseif s == 4
        return MOI.INFEASIBLE_OR_UNBOUNDED
    elseif s == 5
        return MOI.DUAL_INFEASIBLE
    elseif s == 6
        return MOI.INFEASIBLE
    end
end

function MOI.get(optimizer::Optimizer, attr::MOI.ObjectiveValue)
    result_count_err(optimizer, attr)
    value = optimizer.sol.objval
    return value
end
function MOI.get(optimizer::Optimizer, attr::MOI.DualObjectiveValue)
    result_count_err(optimizer, attr)
    value = optimizer.sol.dual_objval
    return value
end

function MOI.get(optimizer::Optimizer, attr::MOI.PrimalStatus)
    s = optimizer.sol.ret_val
    if attr.N > 1 || s == 0
        return MOI.NO_SOLUTION
    end
    if s == 5 && optimizer.sol.certificate_found
        return MOI.INFEASIBILITY_CERTIFICATE
    end
    if optimizer.sol.primal_feasible_user_tol#s == 1
        return MOI.FEASIBLE_POINT
    else
        return MOI.INFEASIBLE_POINT
    end
end
function MOI.get(optimizer::Optimizer, attr::MOI.DualStatus)
    s = optimizer.sol.ret_val
    if attr.N > 1 || s ==0
        return MOI.NO_SOLUTION
    end
    if s == 6 && optimizer.sol.certificate_found
        return MOI.INFEASIBILITY_CERTIFICATE
    end
    if optimizer.sol.dual_feasible_user_tol
        return MOI.FEASIBLE_POINT
    else
        return MOI.INFEASIBLE_POINT
    end
end

function result_count_err(optimizer::Optimizer, attr)
    if optimizer.sol.ret_val == 0
        throw(MOI.ResultIndexBoundsError{typeof(attr)}(attr, 0))
    elseif result_index(attr) > 1
        throw(MOI.ResultIndexBoundsError{typeof(attr)}(attr, 1))
    end
    return nothing
end

result_index(a::MOI.ObjectiveValue) = a.result_index
result_index(a::MOI.DualObjectiveValue) = a.result_index
result_index(a::MOI.ConstraintDual) = a.N
result_index(a::MOI.ConstraintPrimal) = a.N
result_index(a::MOI.VariablePrimal) = a.N

#=
    Solution
=#

function MOI.get(optimizer::Optimizer, attr::MOI.VariablePrimal, vi::VI)
    result_count_err(optimizer, attr)
    optimizer.sol.primal[vi.value]
end
function MOI.get(optimizer::Optimizer, a::MOI.VariablePrimal, vi::Vector{VI})
    result_count_err(optimizer, a)
    return MOI.get.(optimizer, a, vi)
end

function MOI.get(optimizer::Optimizer, attr::MOI.ConstraintPrimal,
                 ci::CI{<:MOI.AbstractFunction, MOI.Zeros})
    result_count_err(optimizer, attr)
    cone = optimizer.cone
    inds = (cone.eq_start[ci.value]-1) .+ (1:cone.eq_len[ci.value])
    return optimizer.sol.slack_eq[inds]
end
function MOI.get(optimizer::Optimizer, attr::MOI.ConstraintPrimal,
    ci::CI{<:MOI.AbstractFunction, MOI.Nonpositives})
    result_count_err(optimizer, attr)
    cone = optimizer.cone
    inds = (cone.in_start[ci.value]-1) .+ (1:cone.in_len[ci.value])
    return optimizer.sol.slack_in[inds]
end

function MOI.get(optimizer::Optimizer, attr::MOI.ConstraintPrimal,
        ci::CI{MOI.VectorOfVariables, MOI.PositiveSemidefiniteConeTriangle})
    result_count_err(optimizer, attr)
    optimizer.sol.primal[optimizer.cone.sdc[ci.value]]
end
function MOI.get(optimizer::Optimizer, attr::MOI.ConstraintPrimal,
    ci::CI{MOI.VectorOfVariables, MOI.SecondOrderCone})
    result_count_err(optimizer, attr)
    optimizer.sol.primal[optimizer.cone.soc[ci.value]]
end

function MOI.get(optimizer::Optimizer, attr::MOI.ConstraintDual,
    ci::CI{<:MOI.AbstractFunction, MOI.Zeros})
    result_count_err(optimizer, attr)
    cone = optimizer.cone
    inds = (cone.eq_start[ci.value]-1) .+ (1:cone.eq_len[ci.value])
    return -optimizer.sol.dual_eq[inds]
end
function MOI.get(optimizer::Optimizer, attr::MOI.ConstraintDual,
ci::CI{<:MOI.AbstractFunction, MOI.Nonpositives})
    result_count_err(optimizer, attr)
    cone = optimizer.cone
    inds = (cone.in_start[ci.value]-1) .+ (1:cone.in_len[ci.value])
    return -optimizer.sol.dual_in[inds]
end

function MOI.get(optimizer::Optimizer, attr::MOI.ConstraintDual,
    ci::CI{MOI.VectorOfVariables, MOI.PositiveSemidefiniteConeTriangle})
    result_count_err(optimizer, attr)
    optimizer.sol.dual_cone[optimizer.cone.sdc[ci.value]]
end
function MOI.get(optimizer::Optimizer, attr::MOI.ConstraintDual,
    ci::CI{MOI.VectorOfVariables, MOI.SecondOrderCone})
    result_count_err(optimizer, attr)
    optimizer.sol.dual_cone[optimizer.cone.soc[ci.value]]
end

function MOI.get(optimizer::Optimizer, ::MOI.ResultCount)
    if MOI.get(optimizer, MOI.TerminationStatus()) in [
                # MOI.INFEASIBLE_OR_UNBOUNDED,
                # MOI.INFEASIBLE,
                # MOI.DUAL_INFEASIBLE,
                MOI.OPTIMIZE_NOT_CALLED
        ]
        return 0
    else
        return 1
    end
end


text:
n = rand(1:10)
@test matrixdepot("neumann", n) == matrixdepot("neumann", Float64, n)

if n == 1
    n = rand(2:10)
end

n_mesh, n = n, n^2

B = zeros(Float64, n, n)

i = 0

for i_mesh in 1:n_mesh, j_mesh in 1:n_mesh
    i = i + 1
    if i_mesh > 1
        j = i - n_mesh
    else
        j = i + n_mesh
    end

    B[i,j] = B[i,j] - 1

    if j_mesh > 1
        j = i - 1
    else
        j = i + 1
    end

    B[i,j] = B[i,j] - 1

    j = i
    B[i,j] = 4

    if j_mesh < n_mesh
        j = i + 1
    else
        j = i - 1
    end

    B[i,j] = B[i,j] - 1

    if i_mesh < n_mesh
        j = i + n_mesh
    else
        j = i - n_mesh
    end

    B[i,j] = B[i,j] - 1
end

A = matrixdepot("neumann", n_mesh)

@test_approx_eq full(A)  B

println("'neumann' passed test...")


text:
using StochasticDiffEq, Test

function f(du, u, p, t)
    du[1] = u[1]
end
function g(du, u, p, t)
    0.0
end

u0 = [1.0]
prob = SDEProblem{true}(f, g, u0, (0.0, 0.1))

sol_ito = solve(prob, RKMil{:Ito}())
@test length(sol_ito) < 100

sol_strato = solve(prob, RKMil{:Stratonovich}(); dt=1e-2)
@test length(sol_strato) < 100

sol_leh = solve(prob, LambaEulerHeun())
@test length(sol_leh) < 100


text:
# Operators and mathematical functions overloaded for UnitDuals

# Binary operators for UnitDuals
+(a::UnitDual, b::UnitDual) = UnitDual(value(a) + value(b), add_partials(partials(a), partials(b)))
-(a::UnitDual, b::UnitDual) = UnitDual(value(a) - value(b), sub_partials(partials(a), partials(b)))
-(a::UnitDual) = UnitDual(-value(a), neg_partials(partials(a)))
*(a::UnitDual, b::UnitDual) = UnitDual(value(a)*value(b), mul_partials(partials(a), partials(b), value(b), value(a)))
/(a::UnitDual, b::UnitDual) = UnitDual(value(a)/value(b), mul_partials(partials(a), partials(b), inv(value(b)),-(value(a)/(value(b)*value(b)))))

# Binary operators for UnitDuals & Number
+(a::UnitDual, b::Number) = UnitDual(value(a) + b, partials(a))
+(a::Number, b::UnitDual) = b + a
-(a::UnitDual, b::Number) = UnitDual(value(a) - b, partials(a))
-(a::Number, b::UnitDual) = a + (-b)
*(a::UnitDual, b::Number) = UnitDual(value(a)*b, scale_partials(partials(a), b))
*(a::Number, b::UnitDual) = b*a
/(a::UnitDual, b::Number) = UnitDual(value(a)/b, scale_partials(partials(a), 1/b))
/(a::Number, b::UnitDual) = UnitDual(a/value(b), scale_partials(partials(b), -a/value(b)^2))
inv(a::UnitDual) =  UnitDual(inv(value(a)), scale_partials(partials(a), -1/value(a)^2))
^(a::UnitDual, b::Number) = UnitDual(value(a)^b, scale_partials(partials(a), b*value(a)^(b - 1)))

# Unary mathematical functions using DiffRules
for fun in (:sqrt, :cbrt, :abs, :abs2, :log, :log10, :log2, :exp, :sin, :cos, :tan, :sec, :csc, :cot,
            :acos, :atan, :asec, :acsc, :acot)
    f = :($fun(value(a)))
    fp = DiffRules.diffrule(:Base, fun, :(value(a)))
    @eval function $fun(a::UnitDual)
        UnitDual($f, scale_partials(partials(a), $fp))
    end
end

# Logical operators for UnitDuals
==(a::UnitDual, b::UnitDual) = value(a) == value(b)
==(a::UnitDual, b::Number) = value(a) == b
==(a::Number, b::UnitDual) = a == value(b)
isless(a::UnitDual, b::UnitDual) = isless(value(a), value(b))
isless(a::UnitDual, b::Number) = isless(value(a), b)
isless(a::Number, b::UnitDual) = isless(a, value(b))
isapprox(a::UnitDual, b::UnitDual) = value(a) ≈ value(b) && all(partials(a)[i] ≈ partials(b)[i] for i in 1:length(partials(a)))
isapprox(a::UnitDual, b::Number) = value(a) ≈ b
isapprox(a::Number, b::UnitDual) = a ≈ value(b)


text:
#############################
# Internal Structures
#############################

const MaybeTask = Union{Nothing, Task}

mutable struct QueuePool <: AbstractThreadPool
    inq  :: Channel{Task}
    outq :: Channel{Task}
    cnt  :: Threads.Atomic{Int}

    QueuePool(tids, handler::Function) = begin
        N = length(tids)
        pool = new(Channel{Task}(N), Channel{Task}(N), Threads.Atomic{Int}(0))
        Threads.@threads for tid in 1:Threads.nthreads()
            if tid in tids
                @async handler(pool)
            end
        end
        return pool
    end
end

function _default_handler(pool)
    for t in pool.inq
        schedule(t)
        wait(t)
        put!(pool.outq, t)
        Threads.atomic_sub!(pool.cnt, 1)
    end
end

#############################
# Constructors
#############################

"""
    QueuePool(init_thrd=1, nthrds=Threads.nthreads())

The main QueuePool object. Its API mimics that of a `Channel{Task}`, but each
submitted task is executed on a different thread.  If `allow_primary` is true,
the assigned thread might be the primary, which will interfere with future
thread management for the duration of any heavy-computational (blocking)
processes.  If it is false, all assigned threads will be off of the primary.
Each thread will only be allowed one Task at a time, but each thread will
backfill with the next queued Task immediately on completion of the previous,
without regard to how bust the other threads may be.
"""
function QueuePool(init_thrd::Integer=1, nthrds::Integer=Threads.nthreads())
    thrd0 = min(init_thrd, Threads.nthreads())
    thrd1 = min(thrd0+nthrds-1, Threads.nthreads())
    return QueuePool(thrd0:thrd1, _default_handler)
end

#############################
# QueuePool API
#############################

"""
    Base.put!(pool::QueuePool, t::Task)

Put the task `t` into the pool, blocking until the pool has
an available thread.
"""
function Base.put!(pool::QueuePool, t::Task)
    Threads.atomic_add!(pool.cnt, 1)
    put!(pool.inq, t)
end

"""
    Base.put!(pool::QueuePool, fn::Function, args...)
    Base.put!(fn::Function, pool::QueuePool, args...)

Creates a task that runs `fn(args...)` and adds it to the pool, blocking
until the pool has an available thread.
"""
Base.put!(pool::QueuePool, fn::Function, args...) = Base.put!(pool, Task(()->fn(args...)))
Base.put!(fn::Function, pool::QueuePool, args...) = Base.put!(pool, Task(()->fn(args...)))

"""
    Base.take!(pool::QueuePool) -> Task

Takes the next available completed task from the pool, blocking until a
task is available.
"""
Base.take!(pool::QueuePool) = fetch(take!(pool.outq))

"""
    Base.close(pool::QueuePool)

Shuts down the pool, closing the internal thread handlers.  It is safe
to issue this command after all Tasks have been submitted, regardless of
the Task completion status. If issued while the pool is still active, it
will `yield` until all tasks have been completed.
"""
function Base.close(pool::QueuePool)
    close(pool.inq)
    while pool.cnt[] > 0
        sleep(0.1)
        yield()
    end
    close(pool.outq)
end

"""
    Base.iterate(pool::QueuePool[, state])

Iterates over the completed Tasks, grabbing the next one available
and ending when the pool has been `close`ed.
"""
Base.iterate(pool::QueuePool, state=nothing) = iterate(pool.outq, state)
Base.IteratorSize(::QueuePool) = Base.SizeUnknown()
Base.eltype(::QueuePool) = Task

"""
    ThreadPools.isactive(pool::QueuePool)

Returns `true` if there are queued Tasks anywhere in the pool, either
awaiting execution, executing, or waiting to be retrieved.
"""
isactive(pool::QueuePool) = isready(pool.inq) || isready(pool.outq) || pool.cnt[] > 0

#############################
# Result Iterator
#############################

struct ResultIterator
    pool :: AbstractThreadPool
end

"""
    results(pool::QueuePool) -> result iterator

Returns an iterator over the `fetch`ed results of the pooled tasks.

# Example

```julia
julia> pool = QueuePool();

julia> @async begin
         for i in 1:4
           put!(pool, x -> 2*x, i)
         end
         close(pool)
       end;

julia> for r in results(pool)
         println(r)
       end
6
2
4
8
```
Note that the execution order across the threads is not guaranteed.
"""
results(pool::QueuePool) = ResultIterator(pool)

function Base.iterate(itr::ResultIterator, state=nothing)
    x = iterate(itr.pool.outq, state)
    isnothing(x) && return nothing
    return fetch(x[1]), nothing
end

Base.IteratorSize(::ResultIterator) = Base.SizeUnknown()
Base.IteratorEltype(::ResultIterator) = Base.EltypeUnknown()

#############################
# ThreadPool Interface
#############################

function tmap(pool::QueuePool, fn::Function, itr)
    N = length(itr)
    result = Vector{_detect_type(fn, itr)}(undef, N)
    _fn = (ind, x) -> (ind, fn(x))
    @async begin
        for (ind, item) in enumerate(itr)
            put!(pool, _fn, ind, item)
        end
    end
    for i in 1:N
        (ind, y) = fetch(take!(pool))
        @inbounds result[ind] = y
    end
    return result
end


text:
using ITensors
using ITensorMPOCompression
using Printf
Base.show(io::IO, f::Float64) = @printf(io, "%1.3f", f) #dumb way to control float output
N=10; #10 sites
NNN=7; #Include up to 7th nearest neighbour interactions
sites = siteinds("S=1/2",N);
H=make_transIsing_MPO(sites,NNN);
is_lower_regular_form(H)==true
truncate!(H)
pprint(H[2])
get_Dw(H)
is_lower_regular_form(H)==true
is_orthogonal(H,left)==true


text:
# KKT system updates -------------------------------------------------------
# Set diagonal
function set_aug_diagonal!(kkt::AbstractKKTSystem, ips::InteriorPointSolver)
    kkt.pr_diag .= ips.zl./(ips.x.-ips.xl) .+ ips.zu./(ips.xu.-ips.x)
    fill!(kkt.du_diag, 0.0)
end
function set_aug_diagonal!(kkt::SparseUnreducedKKTSystem, ips::InteriorPointSolver)
    kkt.pr_diag .= 0.0
    kkt.du_diag .= 0.0
    kkt.l_lower .= .-sqrt.(ips.zl_r)
    kkt.u_lower .= .-sqrt.(ips.zu_r)
    kkt.l_diag  .= ips.xl_r .- ips.x_lr
    kkt.u_diag  .= ips.x_ur .- ips.xu_r
end

# Robust restoration
function set_aug_RR!(kkt::AbstractKKTSystem, ips::InteriorPointSolver, RR::RobustRestorer)
    kkt.pr_diag .= ips.zl./(ips.x.-ips.xl) .+ ips.zu./(ips.xu.-ips.x) .+ RR.zeta.*RR.D_R.^2
    kkt.du_diag .= .-RR.pp./RR.zp .- RR.nn./RR.zn
end
function set_aug_RR!(kkt::SparseUnreducedKKTSystem, ips::InteriorPointSolver, RR::RobustRestorer)
    kkt.pr_diag.= RR.zeta.*RR.D_R.^2
    kkt.du_diag.= .-RR.pp./RR.zp.-RR.nn./RR.zn
    kkt.l_lower.=.-sqrt.(ips.zl_r)
    kkt.u_lower.=.-sqrt.(ips.zu_r)
    kkt.l_diag .= ips.xl_r .- ips.x_lr
    kkt.u_diag .= ips.x_ur .- ips.xu_r
end

# Set RHS
function set_aug_rhs!(ips::InteriorPointSolver, kkt::AbstractKKTSystem, c)
    primal(ips.p) .= .-ips.f.+ips.mu./(ips.x.-ips.xl).-ips.mu./(ips.xu.-ips.x).-ips.jacl
    dual(ips.p)   .= .-c
end

function set_aug_rhs!(ips::InteriorPointSolver, kkt::SparseUnreducedKKTSystem, c)
    primal(ips.p) .= .-ips.f.+ips.zl.-ips.zu.-ips.jacl
    dual(ips.p) .= .-c
    dual_lb(ips.p) .= (ips.xl_r-ips.x_lr).*kkt.l_lower .+ ips.mu./kkt.l_lower
    dual_ub(ips.p) .= (ips.xu_r-ips.x_ur).*kkt.u_lower .- ips.mu./kkt.u_lower
end

function set_aug_rhs_ifr!(ips::InteriorPointSolver, kkt::SparseUnreducedKKTSystem,c)
    primal(ips._w1) .= 0.0
    dual(ips._w1) .= .-c
    dual_lb(ips._w1) .= 0.0
    dual_ub(ips._w1) .= 0.0
end

# Set RHS RR
function set_aug_rhs_RR!(
    ips::InteriorPointSolver, kkt::AbstractKKTSystem, RR::RobustRestorer, rho,
)
    primal(ips.p) .= .-RR.f_R.-ips.jacl.+RR.mu_R./(ips.x.-ips.xl).-RR.mu_R./(ips.xu.-ips.x)
    dual(ips.p) .= .-ips.c.+RR.pp.-RR.nn.+(RR.mu_R.-(rho.-ips.l).*RR.pp)./RR.zp.-(RR.mu_R.-(rho.+ips.l).*RR.nn)./RR.zn
end

# Finish
function finish_aug_solve!(ips::InteriorPointSolver, kkt::AbstractKKTSystem, mu)
    dual_lb(ips.d) .= (mu.-ips.zl_r.*ips.dx_lr)./(ips.x_lr.-ips.xl_r).-ips.zl_r
    dual_ub(ips.d) .= (mu.+ips.zu_r.*ips.dx_ur)./(ips.xu_r.-ips.x_ur).-ips.zu_r
end

function finish_aug_solve!(ips::InteriorPointSolver, kkt::SparseUnreducedKKTSystem, mu)
    dual_lb(ips.d) .*= .-kkt.l_lower
    dual_ub(ips.d) .*= kkt.u_lower
    dual_lb(ips.d) .= (mu.-ips.zl_r.*ips.dx_lr)./(ips.x_lr.-ips.xl_r).-ips.zl_r
    dual_ub(ips.d) .= (mu.+ips.zu_r.*ips.dx_ur)./(ips.xu_r.-ips.x_ur).-ips.zu_r
end

# Initial
function set_initial_rhs!(ips::InteriorPointSolver, kkt::AbstractKKTSystem)
    primal(ips.p) .= .-ips.f.+ips.zl.-ips.zu
    dual(ips.p) .= 0.0
end
function set_initial_rhs!(ips::InteriorPointSolver, kkt::SparseUnreducedKKTSystem)
    primal(ips.p) .= .-ips.f.+ips.zl.-ips.zu
    dual(ips.p) .= 0.0
    dual_lb(ips.p) .= 0.0
    dual_ub(ips.p) .= 0.0
end

# Set ifr
function set_aug_rhs_ifr!(ips::InteriorPointSolver, kkt::AbstractKKTSystem)
    primal(ips._w1) .= 0.0
    dual(ips._w1) .= .-ips.c
end

# Finish RR
function finish_aug_solve_RR!(dpp,dnn,dzp,dzn,l,dl,pp,nn,zp,zn,mu_R,rho)
    dpp .= (mu_R.+pp.*dl.-(rho.-l).*pp)./zp
    dnn .= (mu_R.-nn.*dl.-(rho.+l).*nn)./zn
    dzp .= (mu_R.-zp.*dpp)./pp.-zp
    dzn .= (mu_R.-zn.*dnn)./nn.-zn
end

# Kernel functions ---------------------------------------------------------
is_valid(val::Real) = !(isnan(val) || isinf(val))
function is_valid(vec::AbstractArray)
    @inbounds for i=1:length(vec)
        is_valid(vec[i]) || return false
    end
    return true
end
is_valid(args...) = all(is_valid(arg) for arg in args)

function get_varphi(obj_val,x_lr,xl_r,xu_r,x_ur,mu)
    varphi = obj_val
    @simd for i=1:length(x_lr)
        @inbounds xll = x_lr[i]-xl_r[i]
        xll < 0 && return Inf
        varphi -= mu*log(xll)
    end
    @simd for i=1:length(x_ur)
        @inbounds xuu = xu_r[i]-x_ur[i]
        xuu < 0 && return Inf
        varphi -= mu*log(xuu)
    end
    return varphi
end
get_inf_pr(c) = norm(c,Inf)
function get_inf_du(f,zl,zu,jacl,sd)
    inf_du = 0.
    @simd for i=1:length(f)
        @inbounds inf_du = max(inf_du,abs(f[i]-zl[i]+zu[i]+jacl[i]))
    end
    return inf_du/sd
end
function get_inf_compl(x_lr,xl_r,zl_r,xu_r,x_ur,zu_r,mu,sc)
    inf_compl = 0.
    @simd for i=1:length(x_lr)
        @inbounds inf_compl = max(inf_compl,abs((x_lr[i]-xl_r[i])*zl_r[i]-mu))
    end
    @simd for i=1:length(x_ur)
        @inbounds inf_compl = max(inf_compl,abs((xu_r[i]-x_ur[i])*zu_r[i]-mu))
    end
    return inf_compl/sc
end
function get_varphi_d(f,x,xl,xu,dx,mu)
    varphi_d = 0.
    @simd for i=1:length(f)
        @inbounds varphi_d += (f[i] - mu/(x[i]-xl[i]) + mu/(xu[i]-x[i])) *dx[i]
    end
    return varphi_d
end
function get_alpha_max(x,xl,xu,dx,tau)
    alpha_max = 1.
    @simd for i=1:length(x)
        @inbounds dx[i]<0 && (alpha_max=min(alpha_max,(-x[i]+xl[i])*tau/dx[i]))
        @inbounds dx[i]>0 && (alpha_max=min(alpha_max,(-x[i]+xu[i])*tau/dx[i]))
    end
    return alpha_max
end
function get_alpha_z(zl_r,zu_r,dzl,dzu,tau)
    alpha_z = 1.
    @simd for i=1:length(zl_r)
        @inbounds dzl[i]<0 && (alpha_z=min(alpha_z,-zl_r[i]*tau/dzl[i]))
     end
    @simd for i=1:length(zu_r)
        @inbounds dzu[i]<0 && (alpha_z=min(alpha_z,-zu_r[i]*tau/dzu[i]))
    end
    return alpha_z
end
function get_obj_val_R(p,n,D_R,x,x_ref,rho,zeta)
    obj_val_R = 0.
    @simd for i=1:length(p)
        @inbounds obj_val_R += rho*(p[i]+n[i]) .+ zeta/2*D_R[i]^2*(x[i]-x_ref[i])^2
    end
    return obj_val_R
end
get_theta(c) = norm(c,1)
function get_theta_R(c,p,n)
    theta_R = 0.
    @simd for i=1:length(c)
        @inbounds theta_R += abs(c[i]-p[i]+n[i])
    end
    return theta_R
end
function get_inf_pr_R(c,p,n)
    inf_pr_R = 0.
    @simd for i=1:length(c)
        @inbounds inf_pr_R = max(inf_pr_R,abs(c[i]-p[i]+n[i]))
    end
    return inf_pr_R
end
function get_inf_du_R(f_R,l,zl,zu,jacl,zp,zn,rho,sd)
    inf_du_R = 0.
    @simd for i=1:length(zl)
        @inbounds inf_du_R = max(inf_du_R,abs(f_R[i]-zl[i]+zu[i]+jacl[i]))
    end
    @simd for i=1:length(zp)
        @inbounds inf_du_R = max(inf_du_R,abs(rho-l[i]-zp[i]))
    end
    @simd for i=1:length(zn)
        @inbounds inf_du_R = max(inf_du_R,abs(rho+l[i]-zn[i]))
    end
    return inf_du_R/sd
end
function get_inf_compl_R(x_lr,xl_r,zl_r,xu_r,x_ur,zu_r,pp,zp,nn,zn,mu_R,sc)
    inf_compl_R = 0.
    @simd for i=1:length(x_lr)
        @inbounds inf_compl_R = max(inf_compl_R,abs((x_lr[i]-xl_r[i])*zl_r[i]-mu_R))
    end
    @simd for i=1:length(xu_r)
        @inbounds inf_compl_R = max(inf_compl_R,abs((xu_r[i]-x_ur[i])*zu_r[i]-mu_R))
    end
    @simd for i=1:length(pp)
        @inbounds inf_compl_R = max(inf_compl_R,abs(pp[i]*zp[i]-mu_R))
    end
    @simd for i=1:length(nn)
        @inbounds inf_compl_R = max(inf_compl_R,abs(nn[i]*zn[i]-mu_R))
    end
    return inf_compl_R/sc
end
function get_alpha_max_R(x,xl,xu,dx,pp,dpp,nn,dnn,tau_R)
    alpha_max_R = 1.
    @simd for i=1:length(x)
        @inbounds dx[i]<0 && (alpha_max_R=min(alpha_max_R,(-x[i]+xl[i])*tau_R/dx[i]))
        @inbounds dx[i]>0 && (alpha_max_R=min(alpha_max_R,(-x[i]+xu[i])*tau_R/dx[i]))
    end
    @simd for i=1:length(pp)
        @inbounds dpp[i]<0 && (alpha_max_R=min(alpha_max_R,-pp[i]*tau_R/dpp[i]))
    end
    @simd for i=1:length(nn)
        @inbounds dnn[i]<0 && (alpha_max_R=min(alpha_max_R,-nn[i]*tau_R/dnn[i]))
    end
    return alpha_max_R
end
function get_alpha_z_R(zl_r,zu_r,dzl,dzu,zp,dzp,zn,dzn,tau_R)
    alpha_z_R = 1.
    @simd for i=1:length(zl_r)
        @inbounds dzl[i]<0 && (alpha_z_R=min(alpha_z_R,-zl_r[i]*tau_R/dzl[i]))
    end
    @simd for i=1:length(zu_r)
        @inbounds dzu[i]<0 && (alpha_z_R=min(alpha_z_R,-zu_r[i]*tau_R/dzu[i]))
    end
    @simd for i=1:length(zp)
        @inbounds dzp[i]<0 && (alpha_z_R=min(alpha_z_R,-zp[i]*tau_R/dzp[i]))
    end
    @simd for i=1:length(zn)
        @inbounds dzn[i]<0 && (alpha_z_R=min(alpha_z_R,-zn[i]*tau_R/dzn[i]))
    end
    return alpha_z_R
end
function get_varphi_R(obj_val,x_lr,xl_r,xu_r,x_ur,pp,nn,mu_R)
    varphi_R = obj_val
    @simd for i=1:length(x_lr)
        @inbounds xll = x_lr[i]-xl_r[i]
        xll < 0 && return Inf
        varphi_R -= mu_R*log(xll)
    end
    @simd for i=1:length(x_ur)
        @inbounds xuu = xu_r[i]-x_ur[i]
        xuu < 0 && return Inf
        varphi_R -= mu_R*log(xuu)
    end
    @simd for i=1:length(pp)
        @inbounds pp[i] < 0 && return Inf
        @inbounds varphi_R -= mu_R*log(pp[i])
    end
    @simd for i=1:length(pp)
        @inbounds nn[i] < 0 && return Inf
        @inbounds varphi_R -= mu_R*log(nn[i])
    end
    return varphi_R
end
function get_F(c,f,zl,zu,jacl,x_lr,xl_r,zl_r,xu_r,x_ur,zu_r,mu)
    F = 0.
    for i=1:length(c)
        @inbounds F = max(F,c[i])
    end
    for i=1:length(f)
        @inbounds F = max(F,f[i]-zl[i]+zu[i]+jacl[i])
    end
    for i=1:length(x_lr)
        x_lr[i] >= xl_r[i] || return Inf
        zl_r[i] >= 0       || return Inf
        @inbounds F = max(F,(x_lr[i]-xl_r[i])*zl_r[i]-mu)
    end
    for i=1:length(x_ur)
        xu_r[i] >= x_ur[i] || return Inf
        zu_r[i] >= 0       || return Inf
        @inbounds F = max(F,(xu_r[i]-xu_r[i])*zu_r[i]-mu)
    end
    return F
end
function get_varphi_d_R(f_R,x,xl,xu,dx,pp,nn,dpp,dnn,mu_R,rho)
    varphi_d = 0.
    @simd for i=1:length(x)
        @inbounds varphi_d += (f_R[i] - mu_R/(x[i]-xl[i]) + mu_R/(xu[i]-x[i])) *dx[i]
    end
    @simd for i=1:length(pp)
        @inbounds varphi_d += (rho - mu_R/pp[i]) *dpp[i]
    end
    @simd for i=1:length(nn)
        @inbounds varphi_d += (rho - mu_R/nn[i]) *dnn[i]
    end
    return varphi_d
end
function initialize_variables!(x,xl,xu,bound_push,bound_fac)
    @inbounds @simd for i=1:length(x)
        if xl[i]!=-Inf && xu[i]!=Inf
            x[i]=min(xu[i]-min(bound_push*max(1,abs(xu[i])),bound_fac*(xu[i]-xl[i])),
                     max(xl[i]+min(bound_push*max(1,abs(xl[i])),bound_fac*(xu[i]-xl[i])),x[i]))
        elseif xl[i]!=-Inf && xu[i]==Inf
            x[i]=max(xl[i]+bound_push*max(1,abs(xl[i])),x[i])
        elseif xl[i]==-Inf && xu[i]!=Inf
            x[i]=min(xu[i]-bound_push*max(1,abs(xu[i])),x[i])
        end
    end
end

function adjust_boundary!(x_lr,xl_r,x_ur,xu_r,mu)
    adjusted = 0
    c1 = eps(Float64)*mu
    c2= eps(Float64)^(3/4)
    @simd for i=1:length(xl_r)
        @inbounds x_lr[i]-xl_r[i] < c1 && (xl_r[i] -= c2*max(1,abs(x_lr[i]));adjusted+=1)
    end
    @simd for i=1:length(xu_r)
        @inbounds xu_r[i]-x_ur[i] < c1 && (xu_r[i] += c2*max(1,abs(x_ur[i]));adjusted+=1)
    end
    return adjusted
end
function get_rel_search_norm(x,dx)
    rel_search_norm = 0.
    @simd for i=1:length(x)
        @inbounds rel_search_norm = max(rel_search_norm,abs(dx[i])/(1. +abs(x[i])))
    end
    return rel_search_norm
end

# Utility functions
get_sd(l,zl_r,zu_r,s_max) =
    max(s_max,(norm(l,1)+norm(zl_r,1)+norm(zu_r,1)) / max(1,(length(l)+length(zl_r)+length(zu_r))))/s_max
get_sc(zl_r,zu_r,s_max) =
    max(s_max,(norm(zl_r,1)+norm(zu_r,1)) / max(1,length(zl_r)+length(zu_r)))/s_max
get_mu(mu,mu_min,mu_linear_decrease_factor,mu_superlinear_decrease_power,tol) =
    max(mu_min, max(tol/10,min(mu_linear_decrease_factor*mu,mu^mu_superlinear_decrease_power)))
get_tau(mu,tau_min)=max(tau_min,1-mu)
function get_alpha_min(theta,varphi_d,theta_min,gamma_theta,gamma_phi,alpha_min_frac,del,s_theta,s_phi)
    if varphi_d<0
        if theta<=theta_min
            return alpha_min_frac*min(
                gamma_theta,gamma_phi*theta/(-varphi_d),
                del*theta^s_theta/(-varphi_d)^s_phi)
        else
            return alpha_min_frac*min(
                gamma_theta,gamma_phi*theta/(-varphi_d))
        end
    else
        return alpha_min_frac*gamma_theta
    end
end
is_switching(varphi_d,alpha,s_phi,del,theta,s_theta) = varphi_d < 0 && alpha*(-varphi_d)^s_phi > del*theta^s_theta
is_armijo(varphi_trial,varphi,eta_phi,alpha,varphi_d) = varphi_trial <= varphi + eta_phi*alpha*varphi_d
is_sufficient_progress(theta_trial,theta,gamma_theta,varphi_trial,varphi,gamma_phi,has_constraints) =
    (has_constraints && ((theta_trial<=(1-gamma_theta)*theta+10*eps(Float64)*abs(theta))) ||
    ((varphi_trial<=varphi-gamma_phi*theta +10*eps(Float64)*abs(varphi))))
augment_filter!(filter,theta,varphi,gamma_theta) = push!(filter,((1-gamma_theta)*theta,varphi-gamma_theta*theta))
function is_filter_acceptable(filter,theta,varphi)
    !isnan(theta) || return false
    !isinf(theta) || return false
    !isnan(varphi) || return false
    !isinf(varphi) || return false

    for (theta_F,varphi_F) in filter
        theta <= theta_F || varphi <= varphi_F || return false
    end
    return true
end
is_barr_obj_rapid_increase(varphi,varphi_trial,obj_max_inc) =
    varphi_trial >= varphi && log(10,varphi_trial-varphi) > obj_max_inc + max(1.,log(10,abs(varphi)))
reset_bound_dual!(z,x,mu,kappa_sigma) = (z.=max.(min.(z,kappa_sigma.*mu./x),mu/kappa_sigma./x))
reset_bound_dual!(z,x1,x2,mu,kappa_sigma) = (z.=max.(min.(z,(kappa_sigma*mu)./(x1.-x2)),(mu/kappa_sigma)./(x1.-x2)))
function get_ftype(filter,theta,theta_trial,varphi,varphi_trial,switching_condition,armijo_condition,
                   theta_min,obj_max_inc,gamma_theta,gamma_phi,has_constraints)
    is_filter_acceptable(filter,theta_trial,varphi_trial) || return " "
    !is_barr_obj_rapid_increase(varphi,varphi_trial,obj_max_inc) || return " "

    if theta <= theta_min && switching_condition
        armijo_condition && return "f"
    else
        is_sufficient_progress(
            theta_trial,theta,gamma_theta,varphi_trial,varphi,gamma_phi,has_constraints) && return "h"
    end

    return " "
end

# fixed variable treatment ----------------------------------------------------
function _get_fixed_variable_index(
    mat::SparseMatrixCSC{Tv,Ti1}, ind_fixed::Vector{Ti2}) where {Tv,Ti1,Ti2}

    fixed_aug_index = Int[]
    for i in ind_fixed
        append!(fixed_aug_index,append!(collect(mat.colptr[i]+1:mat.colptr[i+1]-1)))
    end
    append!(fixed_aug_index,setdiff!(Base._findin(mat.rowval,ind_fixed),mat.colptr))

    return fixed_aug_index
end
fixed_variable_treatment_vec!(vec,ind_fixed) = (vec[ind_fixed] .= 0.)
function fixed_variable_treatment_z!(zl,zu,f,jacl,ind_fixed)
    @simd for i in ind_fixed
        z = f[i]+jacl[i]
        z >=0 ? (zl[i] = z; zu[i] = 0.) : (zl[i] = 0.; zu[i] = -z)
    end
end

function dual_inf_perturbation!(px,ind_llb,ind_uub,mu,kappa_d)
    @simd for i in ind_llb
        @inbounds px[i] -= mu*kappa_d
    end
    @simd for i in ind_uub
        @inbounds px[i] += mu*kappa_d
    end
end