text:
__precompile__()

module DrakeLCMTypes

using LCMCore
using StaticArrays: SVector, SMatrix

export body_motion_data_t,
       body_wrench_data_t,
       drake_signal_t,
       driving_control_cmd_t,
       external_force_torque_t,
       foot_flag_t,
       force_torque_t,
       joint_pd_override_t,
       piecewise_polynomial_t,
       polynomial_t,
       polynomial_matrix_t,
       qp_controller_input_t,
       quadrotor_input_t,
       quadrotor_output_t,
       robot_state_t,
       scope_data_t,
       simulation_command_t,
       support_data_t,
       whole_body_data_t,
       zmp_com_observer_state_t,
       zmp_data_t

include("polynomial_t.jl")
include("polynomial_matrix_t.jl")
include("piecewise_polynomial_t.jl")

include("body_wrench_data_t.jl")
include("body_motion_data_t.jl")
include("joint_pd_override_t.jl")
include("support_data_t.jl")
include("whole_body_data_t.jl")
include("zmp_data_t.jl")
include("qp_controller_input_t.jl")

include("drake_signal_t.jl")
include("driving_control_cmd_t.jl")
include("external_force_torque_t.jl")
include("foot_flag_t.jl")
include("force_torque_t.jl")
include("quadrotor_input_t.jl")
include("quadrotor_output_t.jl")
include("robot_state_t.jl")
include("scope_data_t.jl")
include("simulation_command_t.jl")
include("zmp_com_observer_state_t.jl")

end # module


text:
using Base.Test
using AbbrvKW

@AbbrvKW function t1()
    x=1
end
@test t1() == 1

@AbbrvKW function t2(x)
    x
end
@test t2(9) == 9

@AbbrvKW function t3(x, y)
    (x, y)
end
@test t3(3,5) == (3,5)

@AbbrvKW function t4(x, y=12)
    (x, y)
end
@test t4(2) == (2,12)

@AbbrvKW function t5(x, y=12;)
    (x, y)
end
@test t5(3) == (3,12)

@AbbrvKW function t6(x, y=12; k=13)
    (x, y, k)
end
@test t6(1) == (1,12,13)
@test t6(1, k=1) == (1,12,1)

@AbbrvKW function t7(x, y=12; key=13)
    (x, y, key)
end
@test t7(0,1) == (0,1,13)
@test t7(0,2,k=3) == (0,2,3)
@test t7(0,2,ke=3) == (0,2,3)
@test t7(0,2,key=3) == (0,2,3)

@AbbrvKW function t8(x, y=12; key=13, key1=20)
    (x, y, key, key1)
end
@test t8(0) == (0, 12, 13, 20)
@test t8(0, key=-1) == (0, 12, -1, 20)
@test t8(0, key1=-1) == (0, 12, 13, -1)
@test t8(0, key=-1, key1=-2) == (0, 12, -1, -2)
@test_throws MethodError t8(0, ke=0)

@AbbrvKW function t9(x, y=12; key1=13, key2=20)
    (x, y, key1, key2)
end
@test t9(0) == (0, 12, 13, 20)
@test t9(0, key1=-1) == (0, 12, -1, 20)
@test t9(0, key2=-1) == (0, 12, 13, -1)
@test t9(0, key1=-1, key2=-2) == (0, 12, -1, -2)
@test_throws MethodError t9(0, ke=0)

@AbbrvKW function t10(x::Vararg{AbstractArray{T,1},N}; y=12, key=13, aaa=20) where {T,N}
    (y, key, aaa)
end
@test t10([0]) == (12, 13, 20)
@test t10([0], k=1, a=2) == (12, 1, 2)

# Test structure
struct TestStruct1
  foo::Int
  bar::Float64
end

@AbbrvKW function TestFunc(;ABool::Bool=true,
                           AnotherBool::Bool=false,
                           AVector::Vector{Int}=[1,2,3],
                           AString::String="foo",
                           ATuple=(1,2),
                           AStruct::TestStruct1=TestStruct1(0, 3.14),
                           Generic="generic"
                           )

    return (ABool,
            AnotherBool,
            AVector,
            AString,
            ATuple,
            AStruct,
            Generic)
end

a = TestFunc(Gen="baz", AStru=TestStruct1(1, 2), AB=true, Gen="BAZ")
b = (true, false, [1, 2, 3], "foo", (1, 2), TestStruct1(1, 2.0), "BAZ")

for i in 1:length(a)
    @test a[i] == b[i]
end

@test_throws ErrorException a = TestFunc(zzz=1)

@AbbrvKW function Foo(;Keyword::Int=1, verboseLevel::Union{Void,Int}=nothing, kw...)
    println("Keyword: ", Keyword)
    if verboseLevel != nothing
        println("New verbosity level: ", verboseLevel)
        return (Keyword, verboseLevel)
    end
    return Keyword
end

@test Foo() == 1
@test Foo(K=2) == 2
@test Foo(v=3) == (1,3)
@test Foo(ver=3, Keyw=4) == (4,3)
@test Foo(Keywo=4, ve=3) == (4,3)
@test Foo(Keyword=4, ve=3) == (4,3)
@test Foo(K=4, verboseLevel=3) == (4,3)
@test Foo(bar=2) == 1

ext: jl


text:
@testset "CUFFT" begin

if !isdefined(CuArrays, :CUFFT)
@warn "Not testing CUFFT"
else
using CuArrays.CUFFT
@info "Testing CUFFT $(CUFFT.version())"

# notes:
#   plan_bfft does not need separate testing since it is used by plan_ifft

using FFTW

N1 = 8
N2 = 32
N3 = 64
N4 = 8

MYRTOL = 1e-5
MYATOL = 1e-8

# out-of-place
function dotest1(X::AbstractArray{T,N}) where {T <: Complex,N}
    fftw_X = fft(X)
    d_X = CuArray(X)
    p = plan_fft(d_X)
    d_Y = p * d_X
    Y = collect(d_Y)
    @test isapprox(Y, fftw_X, rtol = MYRTOL, atol = MYATOL)

    pinv = plan_ifft(d_Y)
    d_Z = pinv * d_Y
    Z = collect(d_Z)
    @test isapprox(Z, X, rtol = MYRTOL, atol = MYATOL)

    pinv2 = inv(p)
    d_Z = pinv2 * d_Y
    Z = collect(d_Z)
    @test isapprox(Z, X, rtol = MYRTOL, atol = MYATOL)
end

function dotest1(X::AbstractArray{T,N}) where {T <: Real,N}
    fftw_X = rfft(X)
    d_X = CuArray(X)
    p = plan_rfft(d_X)
    d_Y = p * d_X
    Y = collect(d_Y)
    @test isapprox(Y, fftw_X, rtol = MYRTOL, atol = MYATOL)

    pinv = plan_irfft(d_Y,size(X,1))
    d_Z = pinv * d_Y
    Z = collect(d_Z)
    @test isapprox(Z, X, rtol = MYRTOL, atol = MYATOL)

    pinv2 = inv(p)
    d_Z = pinv2 * d_Y
    Z = collect(d_Z)
    @test isapprox(Z, X, rtol = MYRTOL, atol = MYATOL)

    pinv3 = inv(pinv)
    d_W = pinv3 * d_X
    W = collect(d_W)
    @test isapprox(W, Y, rtol = MYRTOL, atol = MYATOL)
end

# in-place
function dotest2(X::AbstractArray{T,N}) where {T <: Complex,N}
    fftw_X = fft(X)
    d_X = CuArray(X)
    p = plan_fft!(d_X)
    p * d_X
    Y = collect(d_X)
    @test isapprox(Y, fftw_X, rtol = MYRTOL, atol = MYATOL)

    pinv = plan_ifft!(d_X)
    pinv * d_X
    Z = collect(d_X)
    @test isapprox(Z, X, rtol = MYRTOL, atol = MYATOL)
end

# no inplace rfft for now

# batch transforms
function dotest3(X::AbstractArray{T,N},region) where {T <: Complex,N}
    fftw_X = fft(X,region)
    d_X = CuArray(X)
    p = plan_fft(d_X,region)
    d_Y = p * d_X
    Y = collect(d_Y)
    @test isapprox(Y, fftw_X, rtol = MYRTOL, atol = MYATOL)

    pinv = plan_ifft(d_Y,region)
    d_Z = pinv * d_Y
    Z = collect(d_Z)
    @test isapprox(Z, X, rtol = MYRTOL, atol = MYATOL)
end

function dotest3(X::AbstractArray{T,N},region) where {T <: Real,N}
    fftw_X = rfft(X,region)
    d_X = CuArray(X)
    p = plan_rfft(d_X,region)
    d_Y = p * d_X
    Y = collect(d_Y)
    @test isapprox(Y, fftw_X, rtol = MYRTOL, atol = MYATOL)

    pinv = plan_irfft(d_Y,size(X,region[1]),region)
    d_Z = pinv * d_Y
    Z = collect(d_Z)
    @test isapprox(Z, X, rtol = MYRTOL, atol = MYATOL)
end

@testset "FFT" for (rtype,ctype) in [(Float32,ComplexF32), (Float64,ComplexF64)]

@testset "1D FFT" begin
    dims = (N1,)
    X = rand(ctype, dims)
    dotest1(X)
end
@testset "1D inplace FFT" begin
    dims = (N1,)
    X = rand(ctype, dims)
    dotest2(X)
end

@testset "2D FFT" begin
    dims = (N1,N2)
    X = rand(ctype, dims)
    dotest1(X)
end
@testset "2D inplace FFT" begin
    dims = (N1,N2)
    X = rand(ctype, dims)
    dotest2(X)
end

@testset "Batch 1D FFT" begin
    dims = (N1,N2)
    X = rand(ctype, dims)
    dotest3(X,1)

    dims = (N1,N2)
    X = rand(ctype, dims)
    dotest3(X,2)

    dims = (N1,N2)
    X = rand(ctype, dims)
    dotest3(X,(1,2))
end

@testset "3D FFT" begin
    dims = (N1,N2,N3)
    X = rand(ctype, dims)
    dotest1(X)
end
@testset "3D inplace FFT" begin
    dims = (N1,N2,N3)
    X = rand(ctype, dims)
    dotest2(X)
end

@testset "Batch 2D FFT (in 3D)" begin
    dims = (N1,N2,N3)
    for region in [(1,2),(2,3),(1,3)]
        X = rand(ctype, dims)
        dotest3(X,region)
    end

    X = rand(ctype, dims)
    @test_throws ArgumentError dotest3(X,(3,1))
end

@testset "Batch 2D FFT (in 4D)" begin
    dims = (N1,N2,N3,N4)
    for region in [(1,2),(1,4),(3,4)]
        X = rand(ctype, dims)
        dotest3(X,region)
    end
    for region in [(1,3),(2,3),(2,4)]
        X = rand(ctype, dims)
        @test_throws ArgumentError dotest3(X,region)
    end

end

@testset "1D real FFT" begin
    X = rand(rtype, N1)
    dotest1(X)
end

@testset "Batch 1D real FFT" begin
    dims = (N1,N2)
    X = rand(rtype, dims)
    dotest3(X,1)

    dims = (N1,N2)
    X = rand(rtype, dims)
    dotest3(X,2)

    dims = (N1,N2)
    X = rand(rtype, dims)
    dotest3(X,(1,2))
end

@testset "2D real FFT" begin
    X = rand(rtype, N1,N2)
    dotest1(X)
end

@testset "Batch 2D real FFT (in 3D)" begin
    dims = (N1,N2,N3)
    for region in [(1,2),(2,3),(1,3)]
        X = rand(rtype, dims)
        dotest3(X,region)
    end

    X = rand(rtype, dims)
    @test_throws ArgumentError dotest3(X,(3,1))
end

@testset "Batch 2D real FFT (in 4D)" begin
    dims = (N1,N2,N3,N4)
    for region in [(1,2),(1,4),(3,4)]
        X = rand(rtype, dims)
        dotest3(X,region)
    end
    for region in [(1,3),(2,3),(2,4)]
        X = rand(rtype, dims)
        @test_throws ArgumentError dotest3(X,region)
    end
end

@testset "3D real FFT" begin
    X = rand(rtype, N1, N2, N3)
    dotest1(X)
end

end # testset FFT

# integer array arguments
function dotest5(X::AbstractArray{T,N}) where {T <: Complex,N}
    fftw_X = fft(X)
    d_X = CuArray(X)
    p = plan_fft(d_X)
    d_Y = p * d_X
    Y = collect(d_Y)
    @test isapprox(Y, fftw_X, rtol = MYRTOL, atol = MYATOL)
    d_Y = fft(d_X)
    Y = collect(d_Y)
    @test isapprox(Y, fftw_X, rtol = MYRTOL, atol = MYATOL)
end

function dotest5(X::AbstractArray{T,N}) where {T <: Real,N}
    fftw_X = rfft(X)
    d_X = CuArray(X)
    p = plan_rfft(d_X)
    d_Y = p * d_X
    Y = collect(d_Y)
    @test isapprox(Y, fftw_X, rtol = MYRTOL, atol = MYATOL)
    d_Y = rfft(d_X)
    Y = collect(d_Y)
    @test isapprox(Y, fftw_X, rtol = MYRTOL, atol = MYATOL)
end

@testset "Int FFT" for (rtype,ctype) in [(Int32,Complex{Int32}), (Int64,Complex{Int64})]

@testset "1D FFT" begin
    dims = (N1,)
    X = rand(ctype, dims)
    dotest5(X)
end

@testset "1D real FFT" begin
    X = rand(rtype, N1)
    dotest5(X)
end

end # testset int FFT

end

end


text:
import Base: read

"`FileFormat` is used for write special `parse` (and `read`) methods on it."
abstract type FileFormat end

"""
`download_file` uses **HTTP.jl** instead of system calls to download files
from the web. It takes the file url as first argument and, optionally, a path to save it.
Keyword arguments (ie. `redirect`, `retry`, `readtimeout`)
are are directly passed to to `HTTP.open` (`HTTP.request`).
Use the `headers` keyword argument to pass a `Dict{String,String}` with the
header information.

```jldoctest
julia> using MIToS.Utils

julia> download_file("http://www.uniprot.org/uniprot/P69905.fasta","seq.fasta",
       headers = Dict("User-Agent" =>
                      "Mozilla/5.0 (compatible; MSIE 7.01; Windows NT 5.0)"),
       redirect=true)
"seq.fasta"

```
"""
function download_file(url::AbstractString, filename::AbstractString;
                       headers::Dict{String,String}=Dict{String,String}(),
                       kargs...)
    kargs = _modify_kargs_for_proxy(url; kargs...)
    HTTP.open("GET", url, headers; kargs...) do stream
        open(filename, "w") do fh
           write(fh, stream)
        end
    end
    filename
end

function download_file(url::AbstractString;
                       headers::Dict{String,String}=Dict{String,String}(),
                       kargs...)
    download_file(url, tempname(); headers=headers, kargs...)
end

"""
Helper function that modifies keyword argument to include a proxy,
    the proxy url is taken from the https_proxy and http_proxy enviromental
    variables.
"""
function _modify_kargs_for_proxy(url; kargs...)
    if startswith(lowercase(url), "http://")
        proxy_env_var = "http_proxy"
    elseif startswith(lowercase(url),"https://")
        proxy_env_var = "https_proxy"
    else
        return kargs
    end
    if !(:proxy in keys(kargs)) && proxy_env_var in keys(ENV)
        kw = Dict()
        for (k,v) in kargs
            kw[k] = v
        end
        kw[:proxy] = ENV[proxy_env_var]
        kargs = pairs(kw)
    end
    kargs
end

"Create an iterable object that will yield each line from a stream **or string**."
lineiterator(string::String) = eachline(IOBuffer(string))
lineiterator(stream::IO)     = eachline(stream)

"""
Returns the `filename`.
Throws an `ErrorException` if the file doesn't exist, or a warning if the file is empty.
"""
function check_file(filename)
    if !isfile(filename)
        throw(ErrorException(string(filename, " doesn't exist!")))
    elseif filesize(filename) == 0
        @warn string(filename, " is empty!")
    end
    filename
end

"Returns `true` if the file exists and isn't empty."
isnotemptyfile(filename) = isfile(filename) && filesize(filename) > 0

# for using with download, since filename doesn't have file extension
function _read(completename::AbstractString,
               filename::AbstractString,
               format::Type{T},
               args...; kargs...) where T <: FileFormat
    check_file(filename)
    if endswith(completename, ".xml.gz") || endswith(completename, ".xml")
        document = parse_file(filename)
        try
            parse(document, T, args...; kargs...)
        finally
            free(document)
        end
    else
        fh = open(filename, "r")
        try
            fh = endswith(completename, ".gz") ? GzipDecompressorStream(fh) : fh
            parse(fh, T, args...; kargs...)
        finally
            close(fh)
        end
    end
end

"""
`read(pathname, FileFormat [, Type [, … ] ] ) -> Type`

This function opens a file in the `pathname` and calls `parse(io, ...)` for
the given `FileFormat` and `Type` on it. If the  `pathname` is an HTTP or FTP URL,
the file is downloaded with `download` in a temporal file.
Gzipped files should end on `.gz`.
"""
function read(completename::AbstractString,
              format::Type{T},
              args...; kargs...) where T <: FileFormat
    if  startswith(completename, "http://")  ||
        startswith(completename, "https://") ||
        startswith(completename, "ftp://")

        filename = download_file(completename)
        try
            _read(completename, filename, T, args...; kargs...)
        finally
            rm(filename)
        end
    else
        # completename and filename are the same
        _read(completename, completename, T, args...; kargs...)
    end
end


text:
#=
        ScalarAffineFunction -in- LessThan
        ScalarAffineFunction -in- GreaterThan
        ScalarAffineFunction -in- EqualTo
        ScalarAffineFunction -in- Interval
=#
constrdict(model::LinQuadOptimizer, ::LCI{LE})  = cmap(model).less_than
constrdict(model::LinQuadOptimizer, ::LCI{GE})  = cmap(model).greater_than
constrdict(model::LinQuadOptimizer, ::LCI{EQ})  = cmap(model).equal_to
constrdict(model::LinQuadOptimizer, ::LCI{IV})  = cmap(model).interval

function MOI.add_constraint(model::LinQuadOptimizer, func::Linear, set::T) where T <: LinSets
    __assert_supported_constraint__(model, Linear, T)
    canonicalized_func = MOIU.canonical(func)
    add_linear_constraint(model, canonicalized_func, set)
    model.last_constraint_reference += 1
    index = LCI{T}(model.last_constraint_reference)
    dict = constrdict(model, index)
    dict[index] = get_number_linear_constraints(model)
    push!(model.constraint_primal_solution, NaN)
    push!(model.constraint_dual_solution, NaN)
    push!(model.constraint_constant, func.constant)
    return index
end

"""
    add_linear_constraint(model::LinQuadOptimizer, func::Linear, set::S)

Add a constraint of type `func`-in-`set` to `model`.
"""
function add_linear_constraint(model::LinQuadOptimizer, func::Linear, set::S) where S <: Union{LE, GE, EQ}
    add_linear_constraint(model, func, backend_type(model, set), MOIU.getconstant(set))
end

function add_linear_constraint(model::LinQuadOptimizer, func::Linear, set::IV)
    columns = [get_column(model, term.variable_index) for term in func.terms]
    coefficients = [term.coefficient for term in func.terms]
    A = CSRMatrix{Float64}([1, length(coefficients) + 1], columns, coefficients)
    add_ranged_constraints!(model, A, [set.lower], [set.upper])
end

function add_linear_constraint(model::LinQuadOptimizer, func::Linear, sense, rhs::Float64)
    if abs(func.constant) > eps(Float64)
        @warn("Constant in scalar function moved into set.")
    end
    columns = [get_column(model, term.variable_index) for term in func.terms]
    coefficients = [term.coefficient for term in func.terms]
    A = CSRMatrix{Float64}([1, length(coefficients) + 1], columns, coefficients)
    add_linear_constraints!(model, A, [sense], [rhs - func.constant])
end

#=
    Add linear constraints (plural)
=#

function MOI.add_constraints(model::LinQuadOptimizer, func::Vector{Linear},
                             set::Vector{S}) where S <: LinSets
    __assert_supported_constraint__(model, Linear, S)
    @assert length(func) == length(set)
    canonicalized_functions = MOIU.canonical.(func)
    num_existing_constraints = get_number_linear_constraints(model)
    add_linear_constraints(model, canonicalized_functions, set)
    indices = Vector{LCI{S}}(undef, length(canonicalized_functions))
    for (i, foo) in enumerate(canonicalized_functions)
        model.last_constraint_reference += 1
        index = LCI{S}(model.last_constraint_reference)
        dict = constrdict(model, index)
        dict[index] = num_existing_constraints + i
        push!(model.constraint_primal_solution, NaN)
        push!(model.constraint_dual_solution, NaN)
        push!(model.constraint_constant, foo.constant)
        indices[i] = index
    end
    return indices
end

function add_linear_constraints(model::LinQuadOptimizer, functions::Vector{Linear},
                                sets::Vector{S}) where S <: LinSets
    return add_linear_constraints(model, functions,
                                  backend_type.(Ref(model), sets),
                                  MOIU.getconstant.(sets))
end

"""
    functions_to_CSRMatrix(model::LinQuadOptimizer, functions::Vector{Linear}, num_non_zeros::Int)

Convert a vector of ScalarAffineFunctions into a CSRMatrix with `num_non_zeros`
non-zero coefficients.
"""
function functions_to_CSRMatrix(model::LinQuadOptimizer, functions::Vector{Linear}, num_non_zeros::Int)
    columns = Vector{Int}(undef, num_non_zeros)
    coefficients  = Vector{Float64}(undef, num_non_zeros)
    # Compute the row pointers in the compressed sparse row matrix. The row
    # pointers are defined recursively:
    #  r[1] = 1
    #  r[i] = r[i - 1] + (number of nonzero elements in the (i - 1)th row)
    #
    # To compute this, we first count up the number of nonzero elements in each
    # row (i - 1), storing the result in r[i]. Then we perform a cumsum on r,
    # storing the result back in r.
    num_rows = length(functions)
    row_pointers = fill(0, num_rows+1)
    row_pointers[1] = 1
    non_zero_index = 0
    for (row, func) in enumerate(functions)
        for term in func.terms
            non_zero_index += 1
            if non_zero_index > num_non_zeros
                error("There were more non-zero entries in the function than " *
                      "indicated: >$(num_non_zeros).")
            end
            columns[non_zero_index] = get_column(model, term.variable_index)
            coefficients[non_zero_index] = term.coefficient
            if row < num_rows
                row_pointers[row + 1] += 1
            end
        end
    end
    cumsum!(row_pointers, row_pointers)
    row_pointers[end] = length(coefficients)+1
    return CSRMatrix{Float64}(row_pointers, columns, coefficients)
end

function add_linear_constraints(model::LinQuadOptimizer, functions::Vector{Linear}, sets::Vector{IV})
    # loop through once to get number of non-zeros and to move rhs across
    lower_bounds = [set.lower for set in sets]
    upper_bounds = [set.upper for set in sets]
    num_non_zeros = 0
    for (i, func) in enumerate(functions)
        if abs(func.constant) > eps(Float64)
            @warn("Constant in scalar function moved into set.")
            lower_bounds[i] -= func.constant
            upper_bounds[i] -= func.constant
        end
        num_non_zeros += length(func.terms)
    end
    A = functions_to_CSRMatrix(model, functions, num_non_zeros)
    add_ranged_constraints!(model, A, lower_bounds, upper_bounds)
end

function add_linear_constraints(model::LinQuadOptimizer, functions::Vector{Linear},
                                senses::Vector, right_hand_sides::Vector{Float64})
    # loop through once to get number of non-zeros and to move rhs across
    nnz = 0
    for (i, func) in enumerate(functions)
        if abs(func.constant) > eps(Float64)
            @warn("Constant in scalar function moved into set.")
            right_hand_sides[i] -= func.constant
        end
        nnz += length(func.terms)
    end
    A = functions_to_CSRMatrix(model, functions, nnz)
    add_linear_constraints!(model, A, senses, right_hand_sides)
end

#=
    Constraint set of Linear function
=#

function MOI.get(model::LinQuadOptimizer, ::MOI.ConstraintSet, index::LCI{S}) where S <: Union{LE, GE, EQ}
    row = model[index]
    rhs = get_rhs(model, row)
    S(rhs + model.constraint_constant[row])
end

function MOI.get(model::LinQuadOptimizer, ::MOI.ConstraintSet, index::LCI{IV})
    row = model[index]
    lowerbound, upperbound = get_range(model, row)
    return IV(lowerbound + model.constraint_constant[row],
              upperbound + model.constraint_constant[row])
end

#=
    Constraint function of Linear function
=#

function MOI.get(model::LinQuadOptimizer, ::MOI.ConstraintFunction, index::LCI{<: LinSets})
    row = model[index]
    columns, coefficients = get_linear_constraint(model, row)
    terms = map(
        (variable, coefficient)->MOI.ScalarAffineTerm{Float64}(coefficient, variable),
        model.variable_references[columns],
        coefficients
    )
    Linear(terms, model.constraint_constant[row])
end

#=
    Scalar Coefficient Change of Linear Constraint
=#

function MOI.modify(model::LinQuadOptimizer, index::LCI{S}, change::MOI.ScalarCoefficientChange{Float64}) where S <: LinSets
    row = model[index]
    column = get_column(model, change.variable)
    change_matrix_coefficient!(model, row, column, change.new_coefficient)
end

"""
    _replace_with_matching_sparsity!(model::LinQuadOptimizer,
        previous::Linear, replacement::Linear, row)

Internal function, not intended for external use.

Change the linear constraint function at index `row` in `model` from
`previous` to `replacement`. This function assumes that `previous` and
`replacement` have exactly the same sparsity pattern w.r.t. which variables
they include and that both constraint functions are in canonical form (as
returned by `MOIU.canonical()`. Neither assumption is checked within the body
of this function.
"""
function _replace_with_matching_sparsity!(model::LinQuadOptimizer, previous::Linear, replacement::Linear, row)
    rows = fill(row, length(replacement.terms))
    cols = [model.variable_mapping[t.variable_index] for t in replacement.terms]
    coefs = MOI.coefficient.(replacement.terms)
    change_matrix_coefficients!(model, rows, cols, coefs)
end

"""
    _replace_with_different_sparsity!(model::LinQuadOptimizer,
        previous::Linear, replacement::Linear, row)

Internal function, not intended for external use.

Change the linear constraint function at index `row` in `model` from
`previous` to `replacement`. This function assumes that `previous` and
`replacement` may have different sparsity patterns.

This function (and `_replace_with_matching_sparsity!` above) are necessary
because the LQOI API currently *only* allows linear constraint modification
through the `change_matrix_coefficient!` and `change_matrix_coefficients!`
functions. In order to fully replace a linear constraint, we have to zero out
the current matrix coefficients and then set the new matrix coefficients. When
the sparsity patterns match, the zeroing-out step can be skipped.
"""
function _replace_with_different_sparsity!(model::LinQuadOptimizer, previous::Linear, replacement::Linear, row)
    # First, zero out the old constraint function terms
    rows = fill(row, length(previous.terms))
    cols = [model.variable_mapping[t.variable_index] for t in previous.terms]
    coefs = fill(0.0, length(previous.terms))
    change_matrix_coefficients!(model, rows, cols, coefs)

    # Next, set the new constraint function terms
    rows = fill(row, length(replacement.terms))
    cols = [model.variable_mapping[t.variable_index] for t in replacement.terms]
    coefs = MOI.coefficient.(replacement.terms)
    change_matrix_coefficients!(model, rows, cols, coefs)
end

MOI.supports(::LinQuadOptimizer, ::MOI.ConstraintFunction, ::Type{LCI{S}}) where {S <: Union{LE, GE, EQ}} = true
function MOI.set(model::LinQuadOptimizer, attr::MOI.ConstraintFunction, CI::LCI{S}, replacement::Linear) where {S <: Union{LE, GE, EQ}}
    previous = MOI.get(model, attr, CI)
    MOIU.canonicalize!(previous)
    replacement = MOIU.canonical(replacement)
    # If the previous and replacement constraint functions have exactly
    # the same sparsity pattern, then we can take a faster path by just
    # passing the replacement terms to the model. But if their sparsity
    # patterns differ, then we need to first zero out the previous terms
    # and then set the replacement terms.
    row = model[CI]
    if _matching_sparsity_pattern(previous, replacement)
        _replace_with_matching_sparsity!(model, previous, replacement, row)
    else
        _replace_with_different_sparsity!(model, previous, replacement, row)
    end
    change_rhs_coefficient!(model, row, get_rhs(model, row) - (replacement.constant - previous.constant))
    model.constraint_constant[model[CI]] = replacement.constant
    return
end

#=
    Change RHS of linear constraint without modifying sense
=#
MOI.supports(::LinQuadOptimizer, ::MOI.ConstraintSet, ::Type{LCI{S}}) where S <: LinSets = true
function MOI.set(model::LinQuadOptimizer, ::MOI.ConstraintSet, index::LCI{S},
                  new_set::S) where S <: Union{LE, GE, EQ}
    row = model[index]
    rhs = MOIU.getconstant(new_set) - model.constraint_constant[row]
    change_rhs_coefficient!(model, model[index], rhs)
end

function MOI.set(model::LinQuadOptimizer, ::MOI.ConstraintSet, index::LCI{IV},
                  new_set::IV)
    row = model[index]
    constant = model.constraint_constant[row]
    modify_ranged_constraints!(model, [model[index]],
        [new_set.lower - constant], [new_set.upper - constant])
end

#=
    Delete a linear constraint
=#

function MOI.delete(model::LinQuadOptimizer, index::LCI{<: LinSets})
    __assert_valid__(model, index)
    delete_constraint_name(model, index)
    dict = constrdict(model, index)
    row = dict[index]
    delete_linear_constraints!(model, row, row)
    deleteat!(model.constraint_primal_solution, row)
    deleteat!(model.constraint_dual_solution, row)
    deleteat!(model.constraint_constant, row)
    # shift all the other references
    shift_references_after_delete_affine!(model, row)
    delete!(dict, index)
end

#=
    Transform scalar constraint
=#

function MOI.transform(model::LinQuadOptimizer, index::LCI{S1}, new_set::S2) where S1 where S2 <: Union{LE, GE, EQ}
    __assert_supported_constraint__(model, Linear, S2)
    dict = constrdict(model, index)
    row = dict[index]
    change_linear_constraint_sense!(model, [row], [backend_type(model, new_set)])
    model.last_constraint_reference += 1
    index_2 = LCI{S2}(model.last_constraint_reference)
    dict_2 = constrdict(model, index_2)
    dict_2[index_2] = row
    delete!(dict, index)
    delete_constraint_name(model, index)
    return index_2
end


text:
using ImageFiltering
using PkgBenchmark

include("mapwindow.jl")


text:
module QDLDL

export qdldl, \, solve, solve!, refactor!, update_diagonal!, update_values!, offset_values!, positive_inertia, regularized_entries

using AMD, SparseArrays
using LinearAlgebra: istriu, triu, Diagonal

const QDLDL_UNKNOWN = -1;
const QDLDL_USED   = true;
const QDLDL_UNUSED = false;

struct QDLDLWorkspace{Tf<:AbstractFloat,Ti<:Integer}

    #internal workspace data
    etree::Vector{Ti}
      Lnz::Vector{Ti}
    iwork::Vector{Ti}
    bwork::Vector{Bool}
    fwork::Vector{Tf}

    #L matrix row indices and data
    Ln::Int         #always Int since SparseMatrixCSC does it this way
    Lp::Vector{Ti}
    Li::Vector{Ti}
    Lx::Vector{Tf}

    #D and its inverse
    D::Vector{Tf}
    Dinv::Vector{Tf}

    #number of positive values in D
    positive_inertia::Ref{Ti}

    #The upper triangular matrix factorisation target
    #This is the post ordering PAPt of the original data
    triuA::SparseMatrixCSC{Tf,Ti}

    #mapping from entries in the triu form
    #of the original input to the post ordering
    #triu form used for the factorization
    #this can be used when modifying entries
    #of the data matrix for refactoring
    AtoPAPt::Union{Vector{Ti},Nothing}

    #regularization parameters
    Dsigns::Union{Vector{Ti},Nothing}
    regularize_eps::Tf
    regularize_delta::Tf

    #number of regularized entries in D
    regularize_count::Ref{Ti}

end

function QDLDLWorkspace(triuA::SparseMatrixCSC{Tf,Ti},
                        AtoPAPt::Union{Vector{Ti},Nothing},
                        Dsigns::Union{Vector{Ti},Nothing},
                        regularize_eps::Tf,
                        regularize_delta::Tf
) where {Tf<:AbstractFloat,Ti<:Integer}

    etree  = Vector{Ti}(undef,triuA.n)
    Lnz    = Vector{Ti}(undef,triuA.n)
    iwork  = Vector{Ti}(undef,triuA.n*3)
    bwork  = Vector{Bool}(undef,triuA.n)
    fwork  = Vector{Tf}(undef,triuA.n)

    #compute elimination tree using QDLDL converted code
    sumLnz = QDLDL_etree!(triuA.n,triuA.colptr,triuA.rowval,iwork,Lnz,etree)

    if(sumLnz < 0)
        error("Input matrix is not upper triangular or has an empty column")
    end

    #allocate space for the L matrix row indices and data
    Ln = triuA.n
    Lp = Vector{Ti}(undef,triuA.n + 1)
    Li = Vector{Ti}(undef,sumLnz)
    Lx = Vector{Tf}(undef,sumLnz)

    #allocate for D and D inverse
    D  = Vector{Tf}(undef,triuA.n)
    Dinv = Vector{Tf}(undef,triuA.n)

    #allocate for positive inertia count.  -1 to
    #start since we haven't counted anything yet
    positive_inertia = Ref{Ti}(-1)

    #number of regularized entries in D. None to start
    regularize_count = Ref{Ti}(0)

    QDLDLWorkspace(etree,Lnz,iwork,bwork,fwork,
                   Ln,Lp,Li,Lx,D,Dinv,positive_inertia,triuA,
                   AtoPAPt, Dsigns,regularize_eps,
                   regularize_delta,regularize_count)

end

struct QDLDLFactorisation{Tf<:AbstractFloat,Ti<:Integer}

    #permutation vector (nothing if no permutation)
    perm::Union{Nothing,Vector{Ti}}
    #inverse permutation (nothing if no permutation)
    iperm::Union{Nothing,Vector{Ti}}
    #lower triangular factor
    L::SparseMatrixCSC{Tf,Ti}
    #Inverse of D matrix in ldl
    Dinv::Diagonal{Tf,Vector{Tf}}
    #workspace data
    workspace::QDLDLWorkspace{Tf,Ti}
    #is it logical factorisation only?
    logical::Ref{Bool}
end

# Usage :
# qdldl(A) uses the default AMD ordering
# qdldl(A,perm=p) uses a caller specified ordering
# qdldl(A,perm = nothing) factors without reordering
#
# qdldl(A,logical=true) produces a logical factorisation only
#
# qdldl(A,signs = s, thresh_eps = ϵ, thresh_delta = δ) produces
# a factorization with dynamic regularization based on the vector
# of signs in s and using regularization parameters (ϵ,δ).  The
# scalars (ϵ,δ) = (1e-12,1e-7) by default.   By default s = nothing,
# and no regularization is performed.

function qdldl(A::SparseMatrixCSC{Tf,Ti};
               perm::Union{Array{Ti},Nothing}=amd(A),
               logical::Bool=false,
               Dsigns::Union{Array{Ti},Nothing} = nothing,
               regularize_eps::Tf = Tf(1e-12),
               regularize_delta::Tf = Tf(1e-7)
              ) where {Tf<:AbstractFloat, Ti<:Integer}

    #store the inverse permutation to enable matrix updates
    iperm = perm == nothing ? nothing : invperm(perm)

    if(!istriu(A))
        A = triu(A)
    end

    #permute using symperm, producing a triu matrix to factor
    if perm != nothing
        A, AtoPAPt = permute_symmetric(A, iperm)  #returns an upper triangular matrix
    else
        AtoPAPt = nothing
    end

    #hold an internal copy of the (possibly permuted)
    #vector of signs if one was specified
    if(Dsigns != nothing)
        mysigns = similar(Dsigns)
        if(perm == nothing)
            mysigns .= Dsigns
        else
            permute!(mysigns,Dsigns,perm)
        end
    else
        mysigns = nothing
    end

    #allocate workspace
    workspace = QDLDLWorkspace(A,AtoPAPt,mysigns,regularize_eps,regularize_delta)

    #factor the matrix
    factor!(workspace,logical)

    #make user-friendly factors
    L = SparseMatrixCSC(workspace.Ln,
                        workspace.Ln,
                        workspace.Lp,
                        workspace.Li,
                        workspace.Lx)
    Dinv = Diagonal(workspace.Dinv)

    #Psss a Ref{Bool} to the constructor since QDLDLFactorisation
    #is immutable.   All internal functions will just use a Bool

    return QDLDLFactorisation(perm, iperm, L, Dinv, workspace, Ref{Bool}(logical))

end

function positive_inertia(F::QDLDLFactorisation)
    F.workspace.positive_inertia[]
end

function regularized_entries(F::QDLDLFactorisation)
    F.workspace.regularize_count[]
end

function update_values!(
    F::QDLDLFactorisation,
    indices::Union{AbstractArray{Ti},Ti},
    values::Union{AbstractArray{Tf},Tf},
) where{Ti <: Integer, Tf <: Real}

    triuA   = F.workspace.triuA     #post permutation internal data
    AtoPAPt = F.workspace.AtoPAPt   #mapping from input matrix entries to triuA

    triuA.nzval[AtoPAPt[indices]] .= values

    return nothing
end

function offset_values!(
    F::QDLDLFactorisation,
    indices::AbstractArray{Ti},
    offset::Union{Tf,AbstractArray{Tf}},
    signs::Union{Ti,AbstractArray{Ti}} = 1
) where{Ti <: Integer, Tf <: Real}

    triuA   = F.workspace.triuA     #post permutation internal data
    AtoPAPt = F.workspace.AtoPAPt   #mapping from input matrix entries to triuA

    if(signs === 1)
        triuA.nzval[AtoPAPt[indices]] .+= offset
    else
        triuA.nzval[AtoPAPt[indices]] .+= offset.*signs
    end

    return nothing
end

function update_diagonal!(F::QDLDLFactorisation,indices,scalarValue::Real)
    update_diagonal!(F,indices,[scalarValue])
end

function update_diagonal!(F::QDLDLFactorisation,indices,values)

    (length(values) != length(indices) && length(values) != 1 ) &&
        throw(DimensionMismatch("Index and value arrays must be the same size, or values must be a scalar."))

    triuA = F.workspace.triuA
    invp  = F.iperm
    nvals = length(values)

    #triuA should be full rank and upper triangular, so the diagonal element
    #in each column should always be the last nonzero
    for i in 1:length(indices)
         thecol = invp[indices[i]]
         elidx  = triuA.colptr[thecol+1]-1
         therow = triuA.rowval[elidx]
         therow == thecol || error("triu(A) is missing diagonal entries")
         val = nvals == 1 ? values[1] : values[i]
         triuA.nzval[elidx] = val
    end

end

function Base.:\(F::QDLDLFactorisation,b)
    return solve(F,b)
end

function refactor!(F::QDLDLFactorisation)

    #It never makes sense to call refactor for a logical
    #factorization since it will always be the same.  Calling
    #this function implies that we want a numerical factorization

    F.logical[] = false  #in case not already

    factor!(F.workspace,F.logical[])
end

function factor!(workspace::QDLDLWorkspace{Tf,Ti},logical::Bool) where {Tf<:AbstractFloat,Ti<:Integer}

    if(logical)
        workspace.Lx   .= 1
        workspace.D    .= 1
        workspace.Dinv .= 1
    end

    #factor using QDLDL converted code
    A = workspace.triuA
    posDCount  = QDLDL_factor!(A.n,A.colptr,A.rowval,A.nzval,
                              workspace.Lp,
                              workspace.Li,
                              workspace.Lx,
                              workspace.D,
                              workspace.Dinv,
                              workspace.Lnz,
                              workspace.etree,
                              workspace.bwork,
                              workspace.iwork,
                              workspace.fwork,
                              logical,
                              workspace.Dsigns,
                              workspace.regularize_eps,
                              workspace.regularize_delta,
                              workspace.regularize_count
                              )

    if(posDCount < 0)
        error("Zero entry in D (matrix is not quasidefinite)")
    end

    workspace.positive_inertia[] = posDCount

    return nothing

end

# Solves Ax = b using LDL factors for A.
# Returns x, preserving b
function solve(F::QDLDLFactorisation,b)
    x = copy(b)
    solve!(F,x)
    return x
end

# Solves Ax = b using LDL factors for A.
# Solves in place (x replaces b)
function solve!(F::QDLDLFactorisation,b)

    #bomb if logical factorisation only
    if F.logical[]
        error("Can't solve with logical factorisation only")
    end

    #permute b
    tmp = F.perm == nothing ? b : permute!(F.workspace.fwork,b,F.perm)

    QDLDL_solve!(F.workspace.Ln,
                 F.workspace.Lp,
                 F.workspace.Li,
                 F.workspace.Lx,
                 F.workspace.Dinv,
                 tmp)

    #inverse permutation
    b = F.perm == nothing ? tmp : ipermute!(b,F.workspace.fwork,F.perm)

    return nothing
end

# Compute the elimination tree for a quasidefinite matrix
# in compressed sparse column form.

function QDLDL_etree!(n,Ap,Ai,work,Lnz,etree)

    @inbounds for i = 1:n
        # zero out Lnz and work.  Set all etree values to unknown
        work[i]  = 0
        Lnz[i]   = 0
        etree[i] = QDLDL_UNKNOWN

        #Abort if A doesn't have at least one entry
        #one entry in every column
        if(Ap[i] == Ap[i+1])
            return -1
        end
    end

    @inbounds for j = 1:n
        work[j] = j
        @inbounds for p = Ap[j]:(Ap[j+1]-1)
            i = Ai[p]
            if(i > j)
                return -1
            end
            @inbounds while(work[i] != j)
                if(etree[i] == QDLDL_UNKNOWN)
                    etree[i] = j
                end
                Lnz[i] += 1        #nonzeros in this column
                work[i] = j
                i = etree[i]
            end
        end #end for p
    end

    #tally the total nonzeros
    sumLnz = sum(Lnz)

    return sumLnz
end

function QDLDL_factor!(
        n,Ap,Ai,Ax,Lp,Li,Lx,
        D,Dinv,Lnz,etree,bwork,iwork,fwork,
        logicalFactor::Bool,Dsigns,
        regularize_eps,regularize_delta,regularize_count
)

    positiveValuesInD  = 0
    regularize_count[] = 0

    #partition working memory into pieces
    yMarkers        = bwork
    yIdx            = view(iwork,      1:n)
    elimBuffer      = view(iwork,  (n+1):2*n)
    LNextSpaceInCol = view(iwork,(2*n+1):3*n)
    yVals           = fwork

    Lp[1] = 1 #first column starts at index one / Julia is 1 indexed

    @inbounds for i = 1:n

        #compute L column indices
        Lp[i+1] = Lp[i] + Lnz[i]   #cumsum, total at the end

        # set all Yidx to be 'unused' initially
        #in each column of L, the next available space
        #to start is just the first space in the column
        yMarkers[i]  = QDLDL_UNUSED
        yVals[i]     = 0.0
        D[i]         = 0.0
        LNextSpaceInCol[i] = Lp[i]
    end

    if(!logicalFactor)
        # First element of the diagonal D.
        D[1]     = Ax[1]
        if(Dsigns != nothing && Dsigns[1]*D[1] < regularize_eps)
            D[1] = regularize_delta * Dsigns[1]
            regularize_count[] += 1
        end

        if(D[1] == 0.0) return -1 end
        if(D[1]  > 0.0) positiveValuesInD += 1 end
        Dinv[1] = 1/D[1];
    end

    #Start from second row here. The upper LH corner is trivially 0
    #in L b/c we are only computing the subdiagonal elements
    @inbounds for k = 2:n

        #NB : For each k, we compute a solution to
        #y = L(0:(k-1),0:k-1))\b, where b is the kth
        #column of A that sits above the diagonal.
        #The solution y is then the kth row of L,
        #with an implied '1' at the diagonal entry.

        #number of nonzeros in this row of L
        nnzY = 0  #number of elements in this row

        #This loop determines where nonzeros
        #will go in the kth row of L, but doesn't
        #compute the actual values
        @inbounds for i = Ap[k]:(Ap[k+1]-1)

            bidx = Ai[i]   # we are working on this element of b

            #Initialize D[k] as the element of this column
            #corresponding to the diagonal place.  Don't use
            #this element as part of the elimination step
            #that computes the k^th row of L
            if(bidx == k)
                D[k] = Ax[i];
                continue
            end

            yVals[bidx] = Ax[i]   # initialise y(bidx) = b(bidx)

            # use the forward elimination tree to figure
            # out which elements must be eliminated after
            # this element of b
            nextIdx = bidx

            if(yMarkers[nextIdx] == QDLDL_UNUSED)  #this y term not already visited

                yMarkers[nextIdx] = QDLDL_USED     #I touched this one
                elimBuffer[1]     = nextIdx  # It goes at the start of the current list
                nnzE              = 1         #length of unvisited elimination path from here

                nextIdx = etree[bidx];

                @inbounds while(nextIdx != QDLDL_UNKNOWN && nextIdx < k)
                    if(yMarkers[nextIdx] == QDLDL_USED) break; end

                    yMarkers[nextIdx] = QDLDL_USED;   #I touched this one
                    #NB: Julia is 1-indexed, so I increment nnzE first here,
                    #not after writing into elimBuffer as in the C version
                    nnzE += 1                   #the list is one longer than before
                    elimBuffer[nnzE] = nextIdx; #It goes in the current list
                    nextIdx = etree[nextIdx];   #one step further along tree

                end #end while

                # now I put the buffered elimination list into
                # my current ordering in reverse order
                @inbounds while(nnzE != 0)
                    #NB: inc/dec reordered relative to C because
                    #the arrays are 1 indexed
                    nnzY += 1;
                    yIdx[nnzY] = elimBuffer[nnzE];
                    nnzE -= 1;
                end #end while
            end #end if

        end #end for i

        #This for loop places nonzeros values in the k^th row
        @inbounds for i = nnzY:-1:1

            #which column are we working on?
            cidx = yIdx[i]

            # loop along the elements in this
            # column of L and subtract to solve to y
            tmpIdx = LNextSpaceInCol[cidx];

            #don't compute Lx for logical factorisation
            #this is not implemented in the C version
            if(!logicalFactor)
                yVals_cidx = yVals[cidx]
                @inbounds for j = Lp[cidx]:(tmpIdx-1)
                    yVals[Li[j]] -= Lx[j]*yVals_cidx
                end

                #Now I have the cidx^th element of y = L\b.
                #so compute the corresponding element of
                #this row of L and put it into the right place
                Lx[tmpIdx] = yVals_cidx *Dinv[cidx]

                #D[k] -= yVals[cidx]*yVals[cidx]*Dinv[cidx];
                D[k] -= yVals_cidx*Lx[tmpIdx]
            end

            #also record which row it went into
            Li[tmpIdx] = k

            LNextSpaceInCol[cidx] += 1

            #reset the yvalues and indices back to zero and QDLDL_UNUSED
            #once I'm done with them
            yVals[cidx]     = 0.0
            yMarkers[cidx]  = QDLDL_UNUSED

        end #end for i

        #apply dynamic regularization if a sign
        #vector has been specified.
        if(Dsigns != nothing && Dsigns[k]*D[k] < regularize_eps)
            D[k] = regularize_delta * Dsigns[k]
            regularize_count[] += 1
        end

        #Maintain a count of the positive entries
        #in D.  If we hit a zero, we can't factor
        #this matrix, so abort
        if(D[k] == 0.0) return -1 end
        if(D[k]  > 0.0) positiveValuesInD += 1 end

        #compute the inverse of the diagonal
        Dinv[k]= 1/D[k]

    end #end for k

    return positiveValuesInD

end

# Solves (L+I)x = b, with x replacing b
function QDLDL_Lsolve!(n,Lp,Li,Lx,x)

    @inbounds for i = 1:n
        @inbounds for j = Lp[i]: (Lp[i+1]-1)
            x[Li[j]] -= Lx[j]*x[i];
        end
    end
    return nothing
end

# Solves (L+I)'x = b, with x replacing b
function QDLDL_Ltsolve!(n,Lp,Li,Lx,x)

    @inbounds for i = n:-1:1
        @inbounds for j = Lp[i]:(Lp[i+1]-1)
            x[i] -= Lx[j]*x[Li[j]]
        end
    end
    return nothing
end

# Solves Ax = b where A has given LDL factors,
# with x replacing b
function QDLDL_solve!(n,Lp,Li,Lx,Dinv,b)

    QDLDL_Lsolve!(n,Lp,Li,Lx,b)
    b .*= Dinv;
    QDLDL_Ltsolve!(n,Lp,Li,Lx,b)

end

# internal permutation and inverse permutation
# functions that require no memory allocations
function permute!(x,b,p)
  @inbounds for j = 1:length(x)
      x[j] = b[p[j]];
  end
  return x
end

function ipermute!(x,b,p)
 @inbounds for j = 1:length(x)
     x[p[j]] = b[j];
 end
 return x
end

"Given a sparse symmetric matrix `A` (with only upper triangular entries), return permuted sparse symmetric matrix `P` (only upper triangular) given the inverse permutation vector `iperm`."
function permute_symmetric(
    A::SparseMatrixCSC{Tf, Ti},
    iperm::AbstractVector{Ti},
    Pr::AbstractVector{Ti} = zeros(Ti, nnz(A)),
    Pc::AbstractVector{Ti} = zeros(Ti, size(A, 1) + 1),
    Pv::AbstractVector{Tf} = zeros(Tf, nnz(A))
) where {Tf <: AbstractFloat, Ti <: Integer}

    # perform a number of argument checks
    m, n = size(A)
    m != n && throw(DimensionMismatch("Matrix A must be sparse and square"))

    isperm(iperm) || throw(ArgumentError("pinv must be a permutation"))

    if n != length(iperm)
        throw(DimensionMismatch("Dimensions of sparse matrix A must equal the length of iperm, $((m,n)) != $(iperm)"))
    end

    #we will record a mapping of entries from A to PAPt
    AtoPAPt = zeros(Ti,length(Pv))

    P = _permute_symmetric(A, AtoPAPt, iperm, Pr, Pc, Pv)
    return P, AtoPAPt
end

# the main function without extra argument checks
# following the book: Timothy Davis - Direct Methods for Sparse Linear Systems
function _permute_symmetric(
    A::SparseMatrixCSC{Tf, Ti},
    AtoPAPt::AbstractVector{Ti},
    iperm::AbstractVector{Ti},
    Pr::AbstractVector{Ti},
    Pc::AbstractVector{Ti},
    Pv::AbstractVector{Tf}
) where {Tf <: AbstractFloat, Ti <: Integer}

    # 1. count number of entries that each column of P will have
    n = size(A, 2)
    num_entries = zeros(Ti, n)
    Ar = A.rowval
    Ac = A.colptr
    Av = A.nzval

    # count the number of upper-triangle entries in columns of P, keeping in mind the row permutation
    for colA = 1:n
        colP = iperm[colA]
        # loop over entries of A in column A...
        for row_idx = Ac[colA]:Ac[colA+1]-1
            rowA = Ar[row_idx]
            rowP = iperm[rowA]
            # ...and check if entry is upper triangular
            if rowA <= colA
                # determine to which column the entry belongs after permutation
                col_idx = max(rowP, colP)
                num_entries[col_idx] += one(Ti)
            end
        end
    end
    # 2. calculate permuted Pc = P.colptr from number of entries
    Pc[1] = one(Ti)
    @inbounds for k = 1:n
        Pc[k + 1] = Pc[k] + num_entries[k]

        # reuse this vector memory to keep track of free entries in rowval
        num_entries[k] = Pc[k]
    end
    # use alias
    row_starts = num_entries

    # 3. permute the row entries and position of corresponding nzval
    for colA = 1:n
        colP = iperm[colA]
        # loop over rows of A and determine where each row entry of A should be stored
        for rowA_idx = Ac[colA]:Ac[colA+1]-1
            rowA = Ar[rowA_idx]
            # check if upper triangular
            if rowA <= colA
                rowP = iperm[rowA]
                # determine column to store the entry
                col_idx = max(colP, rowP)

                # find next free location in rowval (this results in unordered columns in the rowval)
                rowP_idx = row_starts[col_idx]

                # store rowval and nzval
                Pr[rowP_idx] = min(colP, rowP)
                Pv[rowP_idx] = Av[rowA_idx]

                #record this into the mapping vector
                AtoPAPt[rowA_idx] = rowP_idx

                # increment next free location
                row_starts[col_idx] += 1
            end
        end
    end
    nz_new = Pc[end] - 1
    P = SparseMatrixCSC{Tf, Ti}(n, n, Pc, Pr[1:nz_new], Pv[1:nz_new])

    return P
end

end #end module


text:
using repeat_devlp
using Test

@testset "repeat_devlp.jl" begin
    # Write your tests here.
end


text:
function print_first_line(stats::Stats, settings::Settings)
    if settings.verbose
        printline = "\n"
        printline *= """
                 -------------------------------------------------
                   SocpSolver v0.0.1 - (c) Nicholas Moehle, 2021
                 -------------------------------------------------
        """
        printline *= @sprintf "iter  "
        printline *= @sprintf "resid  "
        printline *= @sprintf "best  "
        printline *= @sprintf "  μ    "
        printline *= @sprintf "  σ    "
        printline *= @sprintf "  α    "
        printline *= @sprintf " reg   "
        printline *= @sprintf "|  IR iter  | "
        printline *= @sprintf " prec"
        printline *= "\n"
        print(printline)
    end
end

function print_stats_iter(stats::Stats, settings::Settings)
    if settings.verbose
        num_digits = ceil(log10(settings.max_iters))
        i = stats.iters
        printline = ""
        printline *= @sprintf "%4d  " stats.iters
        printline *= @sprintf "%5.0e  " stats.residual[i]
        if stats.best[i]
            printline *= @sprintf " xx   "
        else
            printline *= @sprintf "      "
        end
        printline *= @sprintf "%5.0e  " stats.μ[i]
        printline *= @sprintf "%5.0e  " stats.σ[i]
        printline *= @sprintf "%5.0e  " stats.α_comb[i]
        printline *= @sprintf "%5.0e  " stats.regularization[i]
        if i % 2 == 1
            printline *= @sprintf "/  "
        else
            printline *= @sprintf "\\  "
        end
        printline *= @sprintf "%1d  " stats.ir_iters_cache[i]
        printline *= @sprintf "%1d  " stats.ir_iters_aff[i]
        printline *= @sprintf "%1d  " stats.ir_iters_comb[i]
        if i % 2 == 1
            printline *= @sprintf "\\ "
        else
            printline *= @sprintf "/ "
        end
        printline *= @sprintf " f64 "
        printline *= "\n"
        print(printline)
    end
end