#ДАНО: Робот - в произвольной клетке ограниченного прямоугольной рамкой поля без внутренних перегородок и маркеров.
#РЕЗУЛЬТАТ: Робот - в исходном положении в центре косого креста (в форме X) из маркеров.
function kosoykrest(r::Robot)
    for i in 0:3

        while !isborder(r, HorizonSide(i)) && !isborder(r, HorizonSide((i+1)%4))
            move!(r, HorizonSide(i))
            move!(r, HorizonSide((i+1)%4))
            putmarker!(r)
        end

        while ismarker(r)
            move!(r, HorizonSide((i+2)%4))
            move!(r, HorizonSide((i+2+1)%4))
        end

        if i == 3
            putmarker!(r)
            break
        end
    end
end


#!/bin/env julia

const filename = "julia_latex_symbols"
const funcname = "get_dict"

open("$filename.vim","w") do f
    println(f, "\" This file is autogenerated from the script '$(basename(Base.source_path()))'")
    println(f, "\" The symbols are based on Julia version $VERSION\n")
    println(f, "scriptencoding utf-8\n")
    println(f, "function! $filename#$funcname()\n",
               "  return {\n",
               "    \\ ",
               join([string("'", latex, "': '", unicode, "'") for (latex,unicode) in sort!(collect(Base.REPLCompletions.latex_symbols), by=x->x[2])],
                    ",\n    \\ "),
               "}")
    println(f, "endfunction")
end


module FloatOperators

import MachineGa.ByteWorks
import MachineGa.RawOperators

function nearbytemutation(
    values::Array{Float64,1};
    mutateprob::Float64 = 0.10,
)::Array{Float64,1}
    return RawOperators.nearbytemutation(
        ByteWorks.floatstobytes(values),
        mutateprob = mutateprob,
    ) |> ByteWorks.bytestofloats
end

function randombytemutation(
    values::Array{Float64,1};
    mutateprob::Float64 = 0.10,
)::Array{Float64,1}
    return RawOperators.randombytemutation(
        ByteWorks.floatstobytes(values),
        mutateprob = mutateprob,
    ) |> ByteWorks.bytestofloats
end

function onepointcrossover(
    fvals::Array{Float64,1},
    svals::Array{Float64,1},
)::Array{Float64,1}
    return RawOperators.onepointcrossover(
        ByteWorks.floatstobytes(fvals),
        ByteWorks.floatstobytes(svals),
    ) |> ByteWorks.bytestofloats
end

function uniformcrossover(
    fvals::Array{Float64,1},
    svals::Array{Float64,1},
)::Array{Float64,1}
    return RawOperators.uniformcrossover(
        ByteWorks.floatstobytes(fvals),
        ByteWorks.floatstobytes(svals),
    ) |> ByteWorks.bytestofloats
end

end # end of module


# This file was generated, do not modify it. # hide
Dists.logpdf(Dists.Normal(), 1.0)


#===============================================================================

    SFCModelDef.jl

    struct containing SFC model definition

    Author: Tom Price
    Date:   June 2019

===============================================================================#

import DataStructures.OrderedDict

mutable struct SFCModelDef ## non-linear model, one feature (`x`)
    ## included in SFC_MODEL_BASE
    name            ::SFCModelName
    linear          ::Bool
    _x_strs         ::Vector{String}
    X_strs          ::Vector{String}
    coef_strs       ::Vector{String}
    coef_cnstrnts   ::AbstractVector ## assume all linear
    func_init_coefs ::Function
    pred_strs       ::OrderedDict{Symbol,String}
    ## added by `add*!`` functions
    func_pred_strs  ::OrderedDict{Symbol,String}
    funcs_pred      ::OrderedDict{Symbol,Function}
    func_fit_str    ::String
    func_fit        ::Function
end


MOI.supports(::Optimizer, ::MOI.VariableName, ::Type{MOI.VariableIndex}) = false

MOI.supports_constraint(::Optimizer, ::Type{<:ALLV}, ::Type{<:SS}) = true
MOI.supports_constraint(::Optimizer, ::Type{<:ALLV}, ::Type{<:NCS}) = true

MOI.supports_constraint(::Optimizer, ::Type{<:SF}, ::Type{<:LS}) = true
MOI.supports_constraint(::Optimizer, ::Type{VAF}, ::Type{<:VLS}) = true
MOI.supports_constraint(::Optimizer, ::Type{VOV}, ::Type{<:VLS}) = true

# TODO:
MOI.supports(::Optimizer, ::MOI.ConstraintName, ::Type{MOI.ConstraintIndex}) = false
# TODO we can support those
MOI.supports_constraint(::Optimizer, ::Type{VOV}, ::Type{NPC}) = false

MOI.supports(::Optimizer, ::MOI.VariablePrimalStart, ::Type{MOI.VariableIndex}) = true
MOI.supports(::Optimizer, ::MOI.ConstraintDualStart, ::Type{MOI.ConstraintIndex}) = true

MOI.supports(::Optimizer, ::MOI.NLPBlock) = true

MOI.supports(::Optimizer, ::MOI.ObjectiveFunction{MOI.SingleVariable}) = true
MOI.supports(::Optimizer, ::MOI.ObjectiveFunction{MOI.ScalarAffineFunction{Float64}}) = true
MOI.supports(::Optimizer, ::MOI.ObjectiveFunction{MOI.ScalarQuadraticFunction{Float64}}) = true
MOI.supports(::Optimizer, ::MOI.ObjectiveSense) = true


using BitBasis, LegibleLambdas

export @λ, @lambda

# same with nbatch
Base.length(r::AbstractRegister{B}) where {B} = B

addbits!(n::Int) = @λ(register -> addbits!(register, n))
insert_qubits!(loc::Int; nqubits::Int = 1) =
    @λ(register -> insert_qubits!(register, loc; nqubits = n))

nremain(r::AbstractRegister) = nqubits(r) - nactive(r)
nbatch(r::AbstractRegister{B}) where {B} = B

"""
    focus(f, register, locs...)

Call a callable `f` under the context of `focus`. See also [`focus!`](@ref).

# Example

print the focused register

```julia
julia> r = ArrayReg(bit"101100")
ArrayReg{1,Complex{Float64},Array...}
    active qubits: 6/6

julia> focus(x->(println(x);x), r, 1, 2);
ArrayReg{1,Complex{Float64},Array...}
    active qubits: 2/6
```
"""
focus!(f::Base.Callable, r::AbstractRegister, locs::Int...) = focus(f, r, locs)

"""
    focus!(locs...) -> f(register) -> register

Lazy version of [`focus!`](@ref), this returns a lambda which requires a register.
"""
focus!(locs::Int...) = focus!(locs)
focus!(locs::NTuple{N,Int}) where {N} = @λ(register -> focus!(register, locs))
focus!(locs::UnitRange) = @λ(register -> focus!(register, locs))
focus!(f::Base.Callable, r::AbstractRegister, loc::Int) = focus(f, r, (loc,))
focus!(f::Base.Callable, r::AbstractRegister, locs) =
    relax!(f(focus!(r, locs)), locs; to_nactive = nqubits(r))

relax!(r::AbstractRegister; to_nactive::Int = nqubits(r)) =
    relax!(r, (); to_nactive = to_nactive)

"""
    relax!(locs::Int...; to_nactive=nqubits(register)) -> f(register) -> register

Lazy version of [`relax!`](@ref), it will be evaluated once you feed a register
to its output lambda.
"""
relax!(locs::Int...; to_nactive::Union{Nothing,Int} = nothing) =
    relax!(locs; to_nactive = to_nactive)

function relax!(locs::NTuple{N,Int}; to_nactive::Union{Nothing,Int} = nothing) where {N}
    lambda = function (r::AbstractRegister)
        if to_nactive === nothing
            return relax!(r, locs; to_nactive = nqubits(r))
        else
            return relax!(r, locs; to_nactive = to_nactive)
        end
    end

    @static if VERSION < v"1.1.0"
        return LegibleLambda("(register->relax!(register, locs...; to_nactive))", lambda)
    else
        return LegibleLambda(:(register -> relax!(register, locs...; to_nactive)), lambda)
    end
end

## Measurement
measure!(postprocess::PostProcess, op, reg::AbstractRegister; kwargs...) =
    measure!(postprocess, op, reg, AllLocs(); kwargs...)
measure!(postprocess::PostProcess, reg::AbstractRegister, locs; kwargs...) =
    measure!(postprocess, ComputationalBasis(), reg, locs; kwargs...)
measure!(postprocess::PostProcess, reg::AbstractRegister; kwargs...) =
    measure!(postprocess, ComputationalBasis(), reg, AllLocs(); kwargs...)
measure!(op, reg::AbstractRegister, args...; kwargs...) =
    measure!(NoPostProcess(), op, reg, args...; kwargs...)
measure!(reg::AbstractRegister, args...; kwargs...) =
    measure!(NoPostProcess(), reg, args...; kwargs...)

measure(op, reg::AbstractRegister; kwargs...) = measure(op, reg, AllLocs(); kwargs...)
measure(reg::AbstractRegister, locs; kwargs...) =
    measure(ComputationalBasis(), reg, locs; kwargs...)
measure(reg::AbstractRegister; kwargs...) =
    measure(ComputationalBasis(), reg, AllLocs(); kwargs...)

# focus! to specify locations, we that we only need to consider full-space measure in the future.
function measure!(
    postprocess::PostProcess,
    op,
    reg::AbstractRegister,
    locs;
    kwargs...,
) where {MODE}
    nbit = nactive(reg)
    focus!(reg, locs)
    res = measure!(postprocess, op, reg, AllLocs(); kwargs...)
    if postprocess isa RemoveMeasured
        relax!(reg; to_nactive = nbit - length(locs))
    else
        relax!(reg, locs; to_nactive = nbit)
    end
    res
end

function measure(op, reg::AbstractRegister, locs; kwargs...) where {MODE}
    nbit = nactive(reg)
    focus!(reg, locs)
    res = measure(op, reg, AllLocs(); kwargs...)
    relax!(reg, locs; to_nactive = nbit)
    res
end

"""
    basis(register) -> UnitRange

Returns an `UnitRange` of the all the bits in the Hilbert space of given register.
"""
BitBasis.basis(r::AbstractRegister) = basis(nqubits(r))

invorder!(r::AbstractRegister) = reorder!(r, Tuple(nactive(r):-1:1))

"""
    partial_tr(locs) -> f(register)

Curried version of `partial_tr(register, locs)`.
"""
partial_tr(locs) = @λ(register -> partial_tr(register, locs))

"""
    select!(b::Integer) -> f(register)

Lazy version of [`select!`](@ref). See also [`select`](@ref).
"""
select!(bits...) = @λ(register -> select!(register, bits...))

function Base.iterate(it::AbstractRegister{B}, state = 1) where {B}
    if state > B
        return nothing
    else
        return viewbatch(it, state), state + 1
    end
end

# fallback printing
function Base.show(io::IO, reg::AbstractRegister)
    summary(io, reg)
    print(io, "\n    active qubits: ", nactive(reg), "/", nqubits(reg))
end

ρ(x) = density_matrix(x)


using Base.Test

using ByteParsers: Skip, parsenext, parsedtype, MaybeParsed
using DiscreteRanges: DiscreteRange
using IndirectArrays

using AMDB:
    # errors
    FileError, FileErrors, log_error,
    # autoindexing
    AutoIndex, OrderedCounter,
    # dates
    AMDB_Date, DatePair,
    # utilities
    narrowest_Int, to_narrowest_Int,
    # tuples
    MultiSubs, get_positions

# write your own tests here
@testset "paths" begin
    mktempdir() do dir
        withenv("AMDB_FILES" => dir) do
            @test AMDB.data_directory() == dir
            @test AMDB.data_path("foo") == joinpath(dir, "foo")
            @test AMDB.data_file(2000) ==
                joinpath(dir, "mon_ew_xt_uni_bus_00.csv.gz")
            open(joinpath(dir, "mon_ew_xt_uni_bus.cols.txt"), "w") do io
                write(io, "A;B;C\n")
            end
            @test AMDB.data_colnames() == ["A","B","C"]
        end
    end
end

@testset "error logging and printing" begin
    fe = FileErrors("foo.gz")
    @test repr(fe) == "foo.gz: 0 errors\n"
    @test count(fe) == 0
    log_error(fe, 99, b"bad;bad line", 5)
    @test count(fe) == 1
    @test repr(fe) == "foo.gz: 1 error\nbad;bad line\n    ^ line 99, byte 5\n"
end

@testset "auto indexing" begin
    aa = b"aa"
    bb = b"bb"
    ai = AutoIndex(Vector{UInt8}, Int8)
    @test ai(@view(aa[1:2])) == 1
    @test ai(bb) == 2
    @test ai(aa) == 1
    @test ai(bb) == 2
    @test ai(b"cc") == 3
    @test ai(@view aa[:]) == 1
    @test length(ai) == 3
    @test keys(ai) == [b"aa", b"bb", b"cc"]
    for i in 4:typemax(Int8)
        @test ai(UInt8[1,i]) == i
    end
    @test length(ai) == 127
    # check that we cannot add an element above capacity
    @test_throws AssertionError ai(b"this will not fit")
end

struct AddOne end

@testset "integer narrowing" begin
    ≖(x, y) = false             # also compare types
    ≖(x::Vector{T}, y::Vector{T}) where {T} = isequal(x, y)
    @test to_narrowest_Int([1,2,3]) ≖ Int8[1, 2, 3]
    @test to_narrowest_Int([-129,2,3]) ≖ Int16[-129, 2, 3]
    @test to_narrowest_Int([99, 32768]) ≖ Int32[99, 32768]
end

@testset "multisubs" begin
    m = MultiSubs((1, 3), (x->x^2, x->x^3))
    @test @inferred(m((2, 3, 5))) ≡ (4, 3, 125)
    @test get_positions(m) ≡ (1, 3)
end

@testset "DatePair parsing" begin
    str = b"20010101;20020327;"
    @test parsenext(DatePair(), str, 1, UInt8(';')) ≡
        MaybeParsed(19, DiscreteRange(AMDB_Date(Date(2001, 1, 1)),
                                      AMDB_Date(Date(2002, 3, 27))))
end

@testset "ordered counter" begin
    oc = OrderedCounter{Int, Int32}()
    @test oc(1) == 1
    @test oc(2) == 2
    @test oc(2) == 2
    @test oc(3) == 3
    @test oc(1) == 1
    @test collect(keys(oc)) == Int32[1, 2, 3]
    @test collect(values(oc)) == Int[2, 2, 1]
end

@testset "interning keys" begin
    values = ["foo", "bar", "baz"]
    A = IndirectArray(1:3, values)
    keys = AMDB.intern_keys(A, values)
    all(keys .≡ values)
end


using PyCall
@pyimport pygtsam as gtsam

#file = open("/home/andrej/factor_graph.txt")
file = open("/home/andrej/anpl/code/debugging/ORB_SLAM2_SERIALIZED_GRAPH.txt")

global graph_str, val_str
graph_str = "";
for line in eachline(file)
    #println(line)
    graph_str *= line;
    graph_str *= "\n";
end
close(file)

file = open("/home/andrej/anpl/code/debugging/ORB_SLAM2_SERIALIZED_VALUES.txt")
val_str = "";
for line in eachline(file)
    #println(line)
    val_str *= line;
    val_str *= "\n";
end
close(file)

fg = gtsam.deserializeGraph(graph_str)
fg[:printf]()
#show(IOContext(STDOUT, limit=false), "text/plain", keys(fg))

# get first factor
f0 = fg[:__getitem__](0)

#relPose=gtsam.BetweenFactorPose2;
#relPose = gtsam.getNonlinearFactor(fg, 1)
#z = relPose[:measured]()
#z[:x](), z[:y](), z[:theta]()

initialEst = gtsam.deserializeValues(val_str)
initialEst[:printf]()

# symbol 'x0'
s = gtsam.Symbol( UInt8('x'),0)

robot_poses = gtsam.extractPose3(initialEst)
arr = collect(robot_poses)
# extract the first pose in array, not neccesarily 'x0'
s = arr[1][1]
pose3 = arr[1][2]
s[:print]("")
s[:chr]()
s[:index]()
p = robot_poses[s]
t = p[:translation]()[:vector]()
R = p[:rotation]()[:matrix]()

landmark_pos = gtsam.extractPoint3(initialEst)

# TODO No to_python (by-value) converter found for C++ type: gtsam::Value'
#k = gtsam.extractKeys(initialEst)
#initialEst[:at](k[1])

isam = gtsam.ISAM2()
#isam[:update](fg, initialEst)


#=
# Format specific functions for writing data output files.
=#

function output_values_raw(vars, file)
    # do one variable/component at a time
    if typeof(vars) <: Array
        for vi=1:length(vars) # each variable
            output_values_raw(vars[vi], file);
        end
    else
        # Here vars is just one variable
        N = size(vars.values, 2); # number of points
        for ci=1:size(vars.values,1) # each component for that variable
            file.write(vars.values[ci,:]); # write the values
        end
    end

end

function output_values_csv(vars, file)
    if config.solver_type == FV
        grid = fv_grid;
    else
        grid = grid_data;
    end
    # columns will be like x, y, z, u1,...
    if typeof(vars) <: Array
        N = size(vars[1].values, 2); # number of points (must be same for all vars)
        if vars[1].location == CELL
            x = fv_info.cellCenters;
        else
            x = grid.allnodes;
        end

        # header
        line = "x";
        if dim > 1
            line *= ", y";
        end
        if dim > 2
            line *= ", z";
        end
        for vi=1:length(vars) # each variable
            if size(vars[vi].values,1) > 1
                for ci=1:size(vars[vi].values,1)
                    line *= ", "*string(vars[vi].symbol)*"_"*string(ci);
                end
            else
                line *= ", "*string(vars[vi].symbol);
            end
        end
        println(file, line);

        # data
        for i=1:N
            # x, y, z
            line = string(x[1,i]);
            if dim > 1
                line *= ", "*string(x[2,i]);
            end
            if dim > 2
                line *= ", "*string(x[3,i]);
            end

            # u1, u2, ...
            for vi=1:length(vars) # each variable
                for ci=1:size(vars[vi].values,1)
                    line *= ", "*string(vars[vi].values[ci,i]);
                end
            end
            println(file, line);
        end

    else
        N = size(vars.values, 2); # number of points
        if vars.location == CELL
            x = fv_info.cellCenters;
        else
            x = grid.allnodes;
        end
        dim = size(x,1);

        # header
        line = "x";
        if dim > 1
            line *= ", y";
        end
        if dim > 2
            line *= ", z";
        end
        if size(vars.values,1) > 1
            for ci=1:size(vars.values,1)
                line *= ", "*string(vars.symbol)*"_"*string(ci);
            end
        else
            line *= ", "*string(vars.symbol);
        end
        println(file, line);

        # data
        for i=1:N
            # x, y, z
            line = string(x[1,i]);
            if dim > 1
                line *= ", "*string(x[2,i]);
            end
            if dim > 2
                line *= ", "*string(x[3,i]);
            end

            # u1, u2, ...
            for ci=1:size(vars.values,1)
                line *= ", "*string(vars.values[ci,i]);
            end
            println(file, line);
        end
    end
end

function output_values_vtk(vars, file, ascii)
    if config.solver_type == FV
        grid = fv_grid;
        nel = grid.nel_owned; # Only write owned elements
    else
        grid = grid_data;
        nel = size(grid.loc2glb,2); # all elements
    end
    # Use the WriteVTK package.
    # First build MeshCell array
    if config.dimension == 1
        np_to_type = Dict([(2,3)]); # line
    elseif config.dimension == 2
        np_to_type = Dict([(3,5),(4,9)]); # triangle, quad
    else
        np_to_type = Dict([(4,10),(8,12)]); # tet, hex
    end

    cells = Array{WriteVTK.MeshCell,1}(undef, nel);
    # nodes = fill(-2e20, 3, size(grid.allnodes, 2));
    for ei=1:nel
        nvertex = 0;
        for vi=1:length(grid.glbvertex[:,ei])
            if grid.glbvertex[vi,ei] > 0
                nvertex = nvertex+1;
            else
                break;
            end
        end
        if nvertex < 2
            printerr("Found element with too few vertices when writing VTK output: el="*string(ei)*", vertexmap="*string(grid.glbvertex[:,ei]));
            return;
        end
        cell_type = np_to_type[nvertex];
        cells[ei] = WriteVTK.MeshCell(VTKCellType(cell_type), grid.glbvertex[1:nvertex,ei]);
        # if config.dimension == 1
        #     nodes[1, grid.glbvertex[1:nvertex,ei]] = grid.allnodes[:, grid.glbvertex[1:nvertex,ei]];
        #     nodes[2:3, grid.glbvertex[1:nvertex,ei]] = zeros(2, nvertex);
        # elseif config.dimension == 2
        #     nodes[1:2, grid.glbvertex[1:nvertex,ei]] = grid.allnodes[:, grid.glbvertex[1:nvertex,ei]];
        #     nodes[3, grid.glbvertex[1:nvertex,ei]] = zeros(1, nvertex);
        # else
        #     nodes[:, grid.glbvertex[1:nvertex,ei]] = grid.allnodes[:, grid.glbvertex[1:nvertex,ei]];
        # end
    end

    # Let's try all nodes. If it causes problems, uncomment above to remove ghosts
    if config.dimension == 1
        nodes = zeros(3, size(grid.allnodes, 2));
        nodes[1, :] = grid.allnodes[1, :];
    elseif config.dimension == 2
        nodes = zeros(3, size(grid.allnodes, 2));
        nodes[1:2, :] = grid.allnodes[1:2, :];
    else
        nodes = grid.allnodes;
    end

    # Initialize the file
    # For multiple partitions write a pvtk file
    if config.num_procs > 1
        # # remove ghost nodes (they should have coordinates -2e20)
        # nextind = 1;
        # for ni=1:size(nodes,2)
        #     if !(nodes[1,ni] == -2e20)
        #         nodes[:,nextind] = nodes[:,ni];
        #         nextind += 1;
        #     end
        # end
        # nodes = nodes[:,1:nextind-1];

        vtkfile = pvtk_grid(file, nodes, cells, ascii=ascii, part=config.proc_rank+1, nparts=config.num_procs);
        vtkfile["partition"] = fill(config.partition_index, nel);
    else
        vtkfile = vtk_grid(file, nodes, cells, ascii=ascii);
    end

    # Add values
    if typeof(vars) <: Array
        for vi=1:length(vars)
            if length(vars[vi].symvar) > 1
                for ci=1:length(vars[vi].symvar)
                    compname = string(vars[vi].symbol) * "_" * string(ci);
                    vtkfile[compname] = vars[vi].values[ci,:];
                end
            else
                vtkfile[string(vars[vi].symbol)] = vars[vi].values[:];
            end
        end
    else
        if length(vars.symvar) > 1
            for ci=1:length(vars.symvar)
                compname = string(vars.symbol) * "_" * string(ci);
                vtkfile[compname] = vars.values[ci,:];
            end
        else
            vtkfile[string(vars.symbol)] = vars.values[:];
        end
    end

    return vtk_save(vtkfile);
end

########################################################################################################
# My attempt at writing a vtu file.
function output_values_myvtu(var, file, ascii)
    if config.solver_type == FV
        grid = fv_grid;
    else
        grid = grid_data;
    end

    if !(typeof(var) <: Array)
        vars = [var];
    else
        vars = var;
    end

    cells_only = config.solver_type == FV; # If FV, only use cell data.
    num_points = size(grid.allnodes,2);
    num_cells = grid.nel_owned;
    if ascii
        format = "ascii";
    else
        format = "appended";
    end
    # For appended data
    appended = zeros(UInt64, 0);
    chunk_offset = 1;
    byte_offset = 0;

    # cell types: ??
    nodes_per_element = size(grid.glbvertex,1);
    cell_types = zeros(Int8, num_cells);
    cells_per_line = 16;
    if config.dimension == 1
        for ci=1:num_cells
            cell_types[ci] = 3;
        end
    elseif config.dimension == 2
        for ci=1:num_cells
            if nodes_per_element == 3
                cell_types[ci] = 5;
            elseif nodes_per_element == 4
                cell_types[ci] = 9;
            end
        end
    else
        for ci=1:num_cells
            if nodes_per_element == 4
                cell_types[ci] = 10;
            elseif nodes_per_element == 8
                cell_types[ci] = 12;
            end
        end
    end

    content = "";

    # header
    content *= "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n";
    content *= "<VTKFile type=\"UnstructuredGrid\" version=\"1.0\" byte_order=\"LittleEndian\""*(ascii ? "" : " header_type=\"UInt64\"")*">\n";
    content *= "  <UnstructuredGrid>\n";
    content *= "    <Piece NumberOfPoints=\""*string(num_points)*"\" NumberOfCells=\""*string(num_cells)*"\">\n";

    # Points
    content *= "      <Points>\n";
    if ascii
        content *= "        <DataArray type=\"Float64\" Name=\"Points\" NumberOfComponents=\"3\" format=\"ascii\">\n";
    else
        content *= "        <DataArray type=\"Float64\" Name=\"Points\" NumberOfComponents=\"3\" format=\"appended\" offset=\"0\">\n";
        (chunk_offset, byte_offset) = add_to_appended_data!(appended, chunk_offset, byte_offset, num_points*3 * 8, UInt64);
    end
    for ni=1:num_points
        scoper = chunk_offset + byte_offset;
        if ascii
            content *= "          ";
            content *= string(grid.allnodes[1, ni]) * " ";
            content *= (config.dimension > 1) ? (string(grid.allnodes[2, ni]) * " ") : "0 ";
            content *= (config.dimension > 2) ? (string(grid.allnodes[3, ni]) * " ") : "0 ";
            content *= "\n";
        else
            tmp1 = grid.allnodes[1, ni];
            tmp2 = (config.dimension > 1) ? grid.allnodes[2, ni] : 0.0;
            tmp3 = (config.dimension > 2) ? grid.allnodes[3, ni] : 0.0;
            (chunk_offset, byte_offset) = add_to_appended_data!(appended, chunk_offset, byte_offset, tmp1, Float64);
            (chunk_offset, byte_offset) = add_to_appended_data!(appended, chunk_offset, byte_offset, tmp2, Float64);
            (chunk_offset, byte_offset) = add_to_appended_data!(appended, chunk_offset, byte_offset, tmp3, Float64);
        end
    end
    content *= "        </DataArray>\n";
    content *= "      </Points>\n";

    # Cells
    content *= "      <Cells>\n";
    if ascii
        content *= "        <DataArray type=\"Int32\" Name=\"connectivity\" format=\"ascii\">\n";
    else
        content *= "        <DataArray type=\"Int32\" Name=\"connectivity\" format=\"appended\" offset=\""*string((chunk_offset-1)*8 + byte_offset)*"\">\n";
        (chunk_offset, byte_offset) = add_to_appended_data!(appended, chunk_offset, byte_offset, num_cells*nodes_per_element * 4, UInt64);
    end
    for ci=1:num_cells
        scoper = chunk_offset + byte_offset;
        if ascii
            content *= "          ";
            for ni=1:nodes_per_element
                content *= string(grid.glbvertex[ni, ci]-1) * " ";
            end
            content *= "\n";
        else
            for ni=1:nodes_per_element
                (chunk_offset, byte_offset) = add_to_appended_data!(appended, chunk_offset, byte_offset, grid.glbvertex[ni, ci]-1, Int32);
            end
            #println("cell "*string(ci)*" nn "*string(nodes_per_element)*" co "*string(chunk_offset)*" bo "*string(byte_offset))
        end

    end
    content *= "        </DataArray>\n";

    if ascii
        content *= "        <DataArray type=\"Int32\" Name=\"offsets\" format=\"ascii\">\n";
    else
        #println("doing offsets co "*string(chunk_offset)*" bo "*string(byte_offset)*" -> "*string((chunk_offset-1)*8 + byte_offset))
        content *= "        <DataArray type=\"Int32\" Name=\"offsets\" format=\"appended\" offset=\""*string((chunk_offset-1)*8 + byte_offset)*"\">\n";
        (chunk_offset, byte_offset) = add_to_appended_data!(appended, chunk_offset, byte_offset, num_cells * 4, UInt64);
    end
    tmp = 0;
    offset = 0;
    for ci=1:num_cells
        scoper = chunk_offset + byte_offset;
        if ascii
            if (tmp == 0) content *= "          "; end
            offset += nodes_per_element;
            content *= string(offset) * " ";
            tmp += 1;
            if (tmp == cells_per_line)
                content *= "\n";
                tmp = 0;
            elseif ci == num_cells
                content *= "\n";
            end
        else
            offset += nodes_per_element;
            (chunk_offset, byte_offset) = add_to_appended_data!(appended, chunk_offset, byte_offset, offset, Int32);
            #println("cell "*string(ci)*" off "*string(offset)*" co "*string(chunk_offset)*" bo "*string(byte_offset))
        end
    end
    content *= "        </DataArray>\n";

    if ascii
        content *= "        <DataArray type=\"UInt8\" Name=\"types\" format=\"ascii\">\n";
    else
        #println("doing types co "*string(chunk_offset)*" bo "*string(byte_offset)*" -> "*string((chunk_offset-1)*8 + byte_offset))
        content *= "        <DataArray type=\"UInt8\" Name=\"types\" format=\"appended\" offset=\""*string((chunk_offset-1)*8 + byte_offset)*"\">\n";
        (chunk_offset, byte_offset) = add_to_appended_data!(appended, chunk_offset, byte_offset, num_cells, UInt64);
    end
    tmp = 0;
    for ci=1:num_cells
        scoper = chunk_offset + byte_offset;
        if ascii
            if (tmp == 0) content *= "          "; end
            content *= string(cell_types[ci]) * " ";
            tmp += 1;
            if (tmp == cells_per_line)
                content *= "\n";
                tmp = 0;
            elseif ci == num_cells
                content *= "\n";
            end
        else
            (chunk_offset, byte_offset) = add_to_appended_data!(appended, chunk_offset, byte_offset, cell_types[ci], Int8);
        end
    end
    content *= "        </DataArray>\n";
    content *= "      </Cells>\n";

    # Point data
    content *= "      <PointData>\n";
    if !cells_only
        for vi=1:length(vars)
            scoper = chunk_offset + byte_offset;
            comps = size(vars[vi].values,1);
            if ascii
                content *= "        <DataArray type=\"Float64\" Name=\""*string(vars[vi].symbol)*"\""*
                                " NumberOfComponents=\""*string(comps)*"\"" * " format=\"ascii\">\n";
            else
                content *= "        <DataArray type=\"Float64\" Name=\""*string(vars[vi].symbol)*"\""*
                                " NumberOfComponents=\""*string(comps)*"\"" * " format=\"appended\" offset=\""*string((chunk_offset-1)*8 + byte_offset)*"\">\n";
                (chunk_offset, byte_offset) = add_to_appended_data!(appended, chunk_offset, byte_offset, num_points*comps * 8, UInt64);
            end
            tmp = 0;
            for ni=1:num_points
                if ascii
                    if (tmp == 0) content *= "          "; end
                    if comps > 1
                        for d=1:comps
                            content *= string(vars[vi].values[d, ni]) * " ";
                        end
                        content *= "\n";
                    else
                        content *= string(vars[vi].values[1, ni]) * " ";
                        tmp += 1;
                        if (tmp >= cells_per_line/2)
                            content *= "\n";
                            tmp = 0;
                        elseif ni==num_points
                            content *= "\n";
                        end
                    end
                else
                    for d=1:comps
                        scoper = chunk_offset + byte_offset;
                        (chunk_offset, byte_offset) = add_to_appended_data!(appended, chunk_offset, byte_offset, vars[vi].values[d, ni], Float64);
                    end
                end
            end
            content *= "        </DataArray>\n";
        end
    end
    content *= "      </PointData>\n";

    # Cell data
    content *= "      <CellData>\n";
    if cells_only
        for vi=1:length(vars)
            scoper = chunk_offset + byte_offset;
            comps = size(vars[vi].values,1);
            if ascii
                content *= "        <DataArray type=\"Float64\" Name=\""*string(vars[vi].symbol)*
                                " NumberOfComponents=\""*string(comps)*"\"" * " format=\"ascii\">\n";
            else
                content *= "        <DataArray type=\"Float64\" Name=\""*string(vars[vi].symbol)*
                                " NumberOfComponents=\""*string(comps)*"\"" * " format=\"appended\" offset=\""*string((chunk_offset-1)*8 + byte_offset)*"\">\n";
                (chunk_offset, byte_offset) = add_to_appended_data!(appended, chunk_offset, byte_offset, num_cells*comps * 8, UInt64);
            end
            tmp = 0;
            for ni=1:num_cells
                if ascii
                    if (tmp == 0) content *= "          "; end
                    if comps > 1
                        for d=1:comps
                            content *= string(vars[vi].values[d, ni]) * " ";
                        end
                        content *= "\n";
                    else
                        content *= string(vars[vi].values[1, ni]) * " ";
                        tmp += 1;
                        if (tmp >= cells_per_line/2)
                            content *= "\n";
                            tmp = 0;
                        elseif ni==num_cells
                            content *= "\n";
                        end
                    end
                else
                    for d=1:comps
                        scoper = chunk_offset + byte_offset;
                        (chunk_offset, byte_offset) = add_to_appended_data!(appended, chunk_offset, byte_offset, vars[vi].values[d, ni], Float64);
                    end
                end
            end
            content *= "        </DataArray>\n";
        end
    end
    content *= "      </CellData>\n";

    content *= "    </Piece>\n";
    content *= "  </UnstructuredGrid>\n";

    # appended data if needed
    if !ascii
        content *= "  <AppendedData encoding=\"raw\">\n";
        content *= "_";
        print(file, content);
        for i=1:length(appended)
            write(file, htol(appended[i]));
        end

        content = "\n  </AppendedData>\n";
    end

    # ending
    content *= "</VTKFile>\n";

    println(file, content);
end

# adds one piece of data to an appended data set.
function add_to_appended_data!(data, chunk_offset, byte_offset, newdata, type)
    bytes = UInt64(0);
    tmp = UInt64(0);
    bc = sizeof(type);
    if type == Float64
        intdata = reinterpret(UInt64, newdata)[1];
    elseif type == Float32
        intdata = reinterpret(UInt32, Float32(newdata))[1];
    else
        intdata = newdata;
    end

    # insert the bytes of newdata in little endian
    for i=1:bc
        tmp = (intdata >>> (8*(i-1))) & 0xFF;
        tmp = tmp << (8*(i-1));
        bytes = bytes | tmp;
        tmp = UInt64(0);
    end

    if byte_offset > 0
        lowbc = min(8-byte_offset, bc);
        highbc = bc-lowbc;
        lowtmp = data[chunk_offset];
        hightmp = UInt64(0);

        lowtmp = lowtmp | (bytes << (8*(byte_offset)));

        hightmp = bytes >>> (8*(8-byte_offset));

        data[chunk_offset] = lowtmp;
        if highbc > 0
            push!(data, hightmp);
            return (chunk_offset + 1, highbc);
        else
            byte_offset = byte_offset + lowbc;
            if byte_offset == 8
                chunk_offset += 1;
                byte_offset = 0;
            end
            return (chunk_offset, byte_offset);
        end

    else
        push!(data, bytes);
        if bc == 8
            chunk_offset += 1;
            byte_offset = 0;
        else
            byte_offset = bc;
        end

        return (chunk_offset, byte_offset);
    end
end


function HansenZonalLinear(arg0::jint, arg1::jint)
    return HansenZonalLinear((jint, jint), arg0, arg1)
end

function compute_init_values(obj::HansenZonalLinear, arg0::jdouble)
    return jcall(obj, "computeInitValues", void, (jdouble,), arg0)
end

function get_derivative(obj::HansenZonalLinear, arg0::jint, arg1::jdouble)
    return jcall(obj, "getDerivative", jdouble, (jint, jdouble), arg0, arg1)
end

function get_value(obj::HansenZonalLinear, arg0::jint, arg1::jdouble)
    return jcall(obj, "getValue", jdouble, (jint, jdouble), arg0, arg1)
end


@testset "restrict" begin
    @testset "interfaces" begin
        A = rand(N0f8, 4, 5, 3)

        Ar = @inferred restrict(A, 1)
        @test typeof(Ar) <: Array
        @test size(Ar) == (3, 5, 3)

        Ar = @inferred restrict(A, (1, ))
        @test typeof(Ar) <: Array
        @test size(Ar) == (3, 5, 3)

        Ar = @inferred restrict(A, (1, 2, 3))
        @test typeof(Ar) <: Array
        @test size(Ar) == (3, 3, 2)
        @test Ar == restrict(A)

        @test_throws MethodError restrict(A, 1, 2, 3)
    end

    @testset "numerical test" begin
        A = reshape([UInt16(i) for i = 1:60], 4, 5, 3)
        B = restrict(A, (1,2))
        Btarget = cat([0.96875   4.625   5.96875;
                       2.875    10.5    12.875;
                       1.90625   5.875   6.90625],
                      [8.46875  14.625  13.46875;
                       17.875    30.5    27.875;
                       9.40625  15.875  14.40625],
                      [15.96875  24.625  20.96875;
                       32.875    50.5    42.875;
                       16.90625  25.875  21.90625], dims=3)
        @test B ≈ Btarget

        Argb = reinterpretc(RGB, reinterpret(N0f16, permutedims(A, (3,1,2))))
        B = restrict(Argb)
        Bf = permutedims(reinterpretc(eltype(eltype(B)), B), (2,3,1))
        # isapprox no longer lies, so atol is now serious
        @test isapprox(Bf, Btarget/reinterpret(one(N0f16)), atol=1e-10)

        Argba = reinterpretc(RGBA{N0f16}, reinterpret(N0f16, A))
        B = restrict(Argba)
        @test isapprox(reinterpretc(eltype(eltype(B)), B), restrict(A, (2,3))/reinterpret(one(N0f16)), atol=1e-10)

        A = reshape(1:60, 5, 4, 3)
        B = restrict(A, (1,2,3))
        @test cat([2.6015625   8.71875   6.1171875;
                   4.09375    12.875     8.78125;
                   3.5390625  10.59375   7.0546875],
                  [10.1015625  23.71875  13.6171875;
                   14.09375    32.875    18.78125;
                   11.0390625  25.59375  14.5546875], dims=3) ≈ B
    end

    @testset "singleton dimension" begin
        # https://github.com/JuliaImages/ImageTransformations.jl/issues/47
        @test restrict([1]) == [1.0]
        @test restrict([0 1 0]) == [0.25 0.25]
        @test restrict([0, 1, 0], 5) == [0, 1, 0]
    end

    @testset "OffsetArray" begin
        A = rand(5, 4, 3)
        Ao = OffsetArray(A, (-2,1,0))

        for (dims, offsets) in [
                (1,      (-1, 1, 0)),
                (2,      (-2, 0, 0)),
                ((1, 2), (-1, 0, 0))
            ]
            Ar = @inferred restrict(Ao, dims)
            @test typeof(Ar) <: OffsetArray
            @test Ar.offsets == offsets
            @test parent(Ar) == restrict(A, dims)
        end
    end

    @testset "SubArray" begin
        x0 = [0, 0, 1, 1, 0, 0]
        xv = @view x0[1:2:6]
        x = x0[1:2:6]
        xr = restrict(xv)
        @test xr == restrict(x)
        @test xr isa Array

        x = view(x0, IdentityUnitRange(3:5))
        xr = restrict(x)
        @test xr == restrict(collect(x))
        @test xr isa Array
    end

    @testset "FixedPoint overflow" begin
        # issue https://github.com/JuliaImages/Images.jl/issues/395
        img1 = colorview(RGB, fill(0.9, 3, 5, 5))
        img2 = colorview(RGB, fill(N0f8(0.9), 3, 5, 5))
        @test isapprox(channelview(restrict(img1)), channelview(restrict(img2)), rtol=0.01)
    end

    @testset "Array of arrays" begin
        a = Vector{Int}[[3,3,3], [2,1,7],[-11,4,2]]
        @test restrict(a) == Vector{Float64}[[2,3.5/2,6.5/2], [-5,4.5/2,5.5/2]]
    end

    @testset "various colorant" begin
        # issue https://github.com/JuliaImages/Images.jl/issues/652
        img = testimage("cameraman")
        @test eltype(@inferred(restrict(img))) == Gray{Float32}
        img = testimage("mandrill")
        @test eltype(@inferred(restrict(img))) == RGB{Float32}
        @test eltype(@inferred(restrict(Lab.(img)))) == RGB{Float32}
        img = rand(RGBA{N0f8}, 11, 11)
        @test eltype(@inferred(restrict(img))) == RGBA{Float32}
        @test eltype(@inferred(restrict(LabA.(img)))) == ARGB{Float32}

        ori = repeat(distinguishable_colors(10), inner=(1, 10))
        for T in (
            RGB, BGR, RGBX, XRGB,
            ARGB, RGBA,
            RGB24, ARGB32,
        )
            img = T.(ori)
            out = @inferred restrict(img)
            if T == RGB24
                @test eltype(out) == RGB{Float32}
            elseif T == ARGB32
                @test eltype(out) == ARGB{Float32}
            else
                @test eltype(out) <: T
            end
            ref = restrict(ori)
            @test ref ≈ RGB.(out)
        end
        for T in (Gray, AGray, GrayA, Gray24)
            img = T.(ori)
            out = @inferred restrict(img)
            if T == Gray24
                @test eltype(out) == Gray{Float32}
            else
                @test eltype(out) <: T
            end
            ref = restrict(Gray.(ori))
            @test ref ≈ Gray.(out)
        end
    end
end


# using Revise, FLPDevelopment, BrowseTables

pathMac = "/Volumes/GoogleDrive/My Drive/Flipping/Datasets/Pharmacology/CaspaseDevelopment"
pathLin = "/home/beatriz/mainen.flipping.5ht@gmail.com/Flipping/Datasets/Pharmacology/CaspaseDevelopment"
if ispath(pathMac)
    path = pathMac
elseif ispath(pathLin)
    path = pathLin
else
    error("unknown path")
end
##
Cas_d, Cas_p, Cas_b, Cas_s = process_dataset(path)

for df in (Cas_p, Cas_b, Cas_s)
    df[!,:Virus] = [get(VirusDict,x,"Missing") for x in df.MouseID]
    df[!,:Sex] = [x in females ? "F" : "M" for x in df.MouseID]
    # df[!,:PreInterpoke] = [ismissing(x) ? 0.0 : x for x in df.PreInterpoke]
    transform!(df, :Virus => categorical, renamecols=false)
    gd = groupby(df,:Session)
    transform!(gd, :Streak => maximum => :Performance)
end

levels!(Cas_p.Virus,["tdTomato", "Caspase"])
levels!(Cas_b.Virus,["tdTomato", "Caspase"])
levels!(Cas_s.Virus,["tdTomato", "Caspase"])

for dd in [Cas_s, Cas_p]
    dd[!,:Gen] = [g == "HET" ? "Rbp4-cre" : "Wild Type" for g in dd.Gen]
    dd[!,:Combo] = dd.Gen .* "\n" .* string.(dd.Virus)
    dd[!,:Group] = [r.Gen == "Wild Type" ? "Wild Type" : r.Gen .* "\n" .* string(r.Virus) for r in eachrow(dd)]
end

Afreq = countmap(Cas_s.AfterLast)
Aprob = Dict()
for (a,f) in Afreq
    Aprob[a] = round(f/nrow(Cas_s),digits = 5)
end
Cas_s[!,:IncorrectStart] = [!x for x in Cas_s.CorrectStart]
Cas_s[!,:IncorrectLeave] = [!x for x in Cas_s.CorrectLeave]
Cas_s[!,:P_AfterLast] = [Aprob[a] for a in Cas_s.AfterLast]
gd = groupby(Cas_s,:Session)
transform!(gd, :AfterLast => frequency)
transform!(gd, :AfterLast => (x -> x .<= quantile(x,0.95)) => :Limit)
transform!(gd, :Num_Rewards => cumsum => :Cum_Rewards)
Cas_s[!,:RewRate] = Cas_s.Cum_Rewards ./ Cas_s.Stop
Cas_s[!,:TrialRewRate] = Cas_s.Cum_Rewards ./ Cas_s.Streak
gdp = groupby(Cas_p,[:Session, :Streak])
transform!(gdp,:PokeIn => (x -> x .- x[1]) => :In)
transform!(gdp,[:PokeIn, :PokeOut] => ((x,y) -> y .- x[1]) => :Out)
# for (df,name) in zip([Cas_p, Cas_b, Cas_s],["Pokes", "Bouts", "Streaks"])
#     filename = joinpath(path, "Results_" * string(today()),"FullInfo" * name * ".csv")
#     CSV.write(filename,df)
# end


function POMDPs.requirements_info(solver::AbstractPOMCPSolver, problem::POMDP)
    if @implemented initial_state_distribution(::typeof(problem))
        return requirements_info(solver, problem, initial_state_distribution(problem))
    else
        println("""
        Since POMCP is an online solver, most of the computation occurs in `action(planner, state)`. In order to view the requirements for this function, please, supply a state as the third argument to `requirements_info`, e.g.

            @requirements_info $(typeof(solver))() $(typeof(problem))() $(state_type(typeof(problem)))()

            """)
    end
end

function POMDPs.requirements_info(solver::AbstractPOMCPSolver, problem::POMDP, b)
    policy = solve(solver, problem)
    requirements_info(policy, b)
end

function POMDPs.requirements_info(policy::POMCPPlanner, b)
    @show_requirements action(policy, b)

    problem = policy.problem
    rng = MersenneTwister(1)
    if @implemented(rand(::typeof(rng), ::typeof(b))) &&
        @implemented(actions(::typeof(problem))) &&
        @implemented(iterator(::typeof(actions(problem))))
        s = rand(rng, b)
        a = first(iterator(actions(problem)))
        if @implemented generate_sor(::typeof(policy.problem), ::typeof(s), ::typeof(a), ::typeof(rng))
            sp, o, r = generate_sor(policy.problem, s, a, rng)

            if !isequal(deepcopy(o), o)
                warn("""
                     isequal(deepcopy(o), o) returned false. Is isequal() defined correctly?

                     For POMCP to work correctly, you must define isequal(::$(typeof(o)), ::$(typeof(o))) (see https://docs.julialang.org/en/stable/stdlib/collections/#Associative-Collections-1, https://github.com/andrewcooke/AutoHashEquals.jl#background, also consider using StaticArrays). This warning was thrown because isequal($(deepcopy(o)), $o) returned false.

                     Note: isequal() should also be defined correctly for actions, but no warning will be issued.
                     """)
            end
            if hash(deepcopy(o)) != hash(o)
                warn("""
                     hash(deepcopy(o)) was not equal to hash(o). Is hash() defined correctly?

                     For MCTS to work correctly, you must define hash(::$(typeof(o)), ::UInt) (see https://docs.julialang.org/en/stable/stdlib/collections/#Associative-Collections-1, https://github.com/andrewcooke/AutoHashEquals.jl#background, also consider using StaticArrays). This warning was thrown because hash($(deepcopy(o))) != hash($o).

                     Note: hash() should also be defined correctly for actions, but no warning will be issued.
                     """)
            end
        end
    end
end

@POMDP_require action(p::POMCPPlanner, b) begin
    tree = POMCPTree(p.problem, p.solver.tree_queries)
    @subreq search(p, b, tree)
end

@POMDP_require search(p::POMCPPlanner, b, t::POMCPTree) begin
    P = typeof(p.problem)
    @req rand(::typeof(p.rng), ::typeof(b))
    s = rand(p.rng, b)
    @req isterminal(::P, ::state_type(P))
    @subreq simulate(p, s, POMCPObsNode(t, 1), p.solver.max_depth)
end

@POMDP_require simulate(p::POMCPPlanner, s, hnode::POMCPObsNode, steps::Int) begin
    P = typeof(p.problem)
    S = state_type(P)
    A = action_type(P)
    O = obs_type(P)
    @req generate_sor(::P, ::S, ::A, ::typeof(p.rng))
    @req isequal(::O, ::O)
    @req hash(::O)
    # from insert_obs_node!
    @req n_actions(::P)
    @req actions(::P)
    AS = typeof(actions(p.problem))
    @req iterator(::AS)
    @subreq estimate_value(p.solved_estimator, p.problem, s, hnode, steps)
    @req discount(::P)
end

@POMDP_require estimate_value(f::Function, pomdp::POMDPs.POMDP, start_state, h::BeliefNode, steps::Int) begin
    @req f(::typeof(pomdp), ::typeof(start_state), ::typeof(h), ::typeof(steps))
end


using SEML, Test

make_model("../example/testcase/case3tests.txt", model="FBA")
#sleep(1)
rm("autogeneratedmodel", recursive=true)


# Copyright 2020 Paul Melis (paul.melis@surf.nl)
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#   http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

using Printf
using StaticArrays

include("halfedge.jl")

# Some utility functions to handle Julia's 1-based indexing
function get_vertex(V, idx)
    s = 1+3*(idx-1)
    return SVector(V[s], V[s+1], V[s+2])
end

function set_vertex(V, idx, v)
    s = 1+3*(idx-1)
    V[s:s+2] = v
end

function time_subdivide(vertices::Array, loop_start::Array, loop_total::Array, loops::Array)
    @timev subdivide(vertices, loop_start, loop_total, loops)
end

function subdivide(vertices::Array, loop_start::Array, loop_total::Array, loops::Array)
    #println(vertices)
    #println(loop_start)
    #println(loop_total)
    #println(loops)

    t0 = time()

    num_vertices, num_faces, num_edges, vertex_start_edges, face_start_edges, polygon_edges = build(vertices, loop_start, loop_total, loops)

    #println(num_vertices)
    #println(num_faces)
    #println(num_edges)
    #println(vertex_start_edges)
    #println(face_start_edges)
    #println(polygon_edges)

    t1 = time()
    @printf("(Julia) Building half edges done in %.3fms\n", 1000*(t1-t0))

    println("(Julia) Input: $(num_vertices) vertices, $(num_faces) polygons, $(num_edges) polygon edges")

    # One new vertex for each input face, one new vertex for each edge
    output_num_vertices = num_vertices + num_faces + num_edges
    # 1 .. NV           Original input vertices (initially, are overwritten later on)
    # NV+1 .. NV+NF     New face points
    # NV+NF+1 .. end    New edge points
    output_vertices = Array{Float32}(undef, 3*output_num_vertices)
    # Copy original input vertex positions, to be modified later on
    output_vertices[1:3*num_vertices] = vertices

    # Output is always all quads, as each input face is split into n quads,
    # where n is the number of vertices in the face
    output_num_quads = sum(loop_total)
    output_loop_start = collect(range(UInt32(1), step=UInt32(4), stop=UInt32(output_num_quads*4)))
    output_loop_total = fill(UInt32(4), output_num_quads)
    output_loops = Array{UInt32}(undef, 4*output_num_quads)

    # Wether a vertex is on the boundary
    is_boundary_vertex = zeros(Bool, num_vertices)

    println("(Julia) Output: $(output_num_vertices) vertices, $(output_num_quads) quads")

    function face_point_index(fi)
        return num_vertices + fi
    end

    function edge_point_index(ei)
        return num_vertices + num_faces + ei
    end

    #
    # Subdivide
    #

    # Add new face points: average of existing original face vertices
    for fi = 1:num_faces
        sum = SVector{3, Float32}(0.0, 0.0, 0.0)
        n = 0

        he = start = face_start_edges[fi]
        while true
            sum += get_vertex(vertices, he.source)
            n += 1

            he = he.next
            if he == start break end
        end

        #println("face point ", fi, " ", sum/n)

        set_vertex(output_vertices, face_point_index(fi), sum / n)

    end

    #println("FV ", face_vertices)
    #println("OV ", output_vertices)

    # Add new edge points: average of edge endpoints and neighbouring face points

    for ei = 1:num_edges

        he = polygon_edges[ei]

        if he.sibling != nothing

            edge_point = (
                get_vertex(output_vertices, face_point_index(he.face))
                +
                get_vertex(output_vertices, face_point_index(he.sibling.face))
                +
                get_vertex(output_vertices, he.source)
                +
                get_vertex(output_vertices, he.target)
            ) / 4

        else

            # Boundary rule
            edge_point = 0.5*(get_vertex(output_vertices, he.source) + get_vertex(output_vertices, he.target))

            is_boundary_vertex[he.source] = is_boundary_vertex[he.target] = true

        end

        #println("edge point ", ei, " ", edge_point)

        set_vertex(output_vertices, edge_point_index(ei), edge_point)
    end

    # Move original input vertices to new positions

    for vi = 1:num_vertices

        if !haskey(vertex_start_edges, vi)
            # Skip unconnected vertices
            continue
        end

        P = get_vertex(output_vertices, vi)

        if is_boundary_vertex[vi]

            he = vertex_start_edges[vi]

            Pp = (
                get_vertex(output_vertices, he.target)
                +
                6*P
                +
                get_vertex(output_vertices, he.prev.source)
            ) / 8

            set_vertex(output_vertices, vi, Pp)

        else

            F_sum = SVector{3, Float32}(0, 0, 0)
            R_sum = SVector{3, Float32}(0, 0, 0)
            n = 0

            # R = average of edge midpoints
            #   = 1/n * sum_i(0.5*(P+Q_i))
            #   = 1/n * (0.5*n*P + 0.5*sum_i(Q_i))
            #   = 0.5*P + 1/2n * sum_i(Q_i)

            he = start = vertex_start_edges[vi]
            while true
                F_sum += get_vertex(output_vertices, face_point_index(he.face))
                R_sum += get_vertex(output_vertices, he.target)
                n += 1

                @assert he.sibling != nothing

                he = he.sibling.next
                if he == start break end
            end

            F = F_sum / n
            R = R_sum / (2*n) + 0.5f0 * P

            set_vertex(output_vertices, vi, (F + 2*R + (n-3)*P) / n)

        end
    end

    # Create new face loops for the quads for each subdivided original face

    offs = 1
    ofi = 1

    for fi = 1:num_faces

        he = start = face_start_edges[fi]
        while true
            output_loop_start[ofi] = offs
            output_loops[offs] = edge_point_index(he.index)
            output_loops[offs+1] = he.target
            output_loops[offs+2] = edge_point_index(he.next.index)
            output_loops[offs+3] = face_point_index(he.face)
            offs += 4
            ofi += 1

            he = he.next
            if he == start break end
        end
    end

    t2 = time()
    @printf("(Julia) Subdivision done in %.3fms\n", 1000*(t2-t1))

    return output_vertices, output_loop_start, output_loop_total, output_loops
end


module Cards
# This module generates cards. Its contents will have to be changed to account
# for variations in the card template.

export mk_card

using Printf
using Dates
using Colors
using ColorTypes

# This function returns a function that converts linear indices to Cartesian (x
# and y) indices for the given array.
# Usage: map(ci_mapfunc(xs), collect(enumerate(xs)))
ci_mapfunc(xs) = ((li, x),) -> (CartesianIndices(xs)[li], x)

mk_gridcell((i, n)) = if i.I === (3,3) ""
  else @sprintf("%u %u gat moveto (%02u) show\n", i.I[2]-1, i.I[1]-1, n) end

mk_grid(ns) = map(mk_gridcell ∘ ci_mapfunc(ns), enumerate(ns))

handle_date(date::Nothing)        = ""
handle_date(date::Date)           = handle_date(Dates.format(date,
  dateformat"YYYY-mm-dd"))
handle_date(date::DateTime)       = handle_date(Date(date)) *
  handle_time(Time(date))
handle_date(date::AbstractString) = """
  3 -0.53 ga moveto
  (Date) show
  4 -0.53 ga moveto
  ($(date)) show
  """

handle_time(time::Nothing)        = ""
handle_time(time::Time)           = handle_time(Dates.format(time,
  dateformat"II:MM pp"))
handle_time(time::AbstractString) = """
  3 -0.64 ga moveto
  (Starting At) show
  4 -0.64 ga moveto
  ($(time)) show
  """

# The interface by which card details are passed to this function is subject
# to change.
# (I might make it a struct.)
mk_card(ns, colorant::String; kwargs...) =
  mk_card(ns,
          (convert(RGB, parse(Colorant, colorant)), colorant);
          kwargs...)

mk_card(ns, (col, col_name); num=0, round=0, date=nothing, draft=false) = """
  %!PS
  % Bingo card template
  % Written by Tucker R. Twomey
  /in {72 mul} def

  % Hardware margin
  /hwmargin  0.1389 in            def

  % Get paper dimensions
  currentpagedevice /PageSize get
  dup
  /width    exch 0 get def
  /height   exch 1 get def

  /DeviceCMYK setcolorspace
  % C M   Y   K
  0.0 0.0 0.0 1.0 setcolor

  % Function to draw box
  /rect
  {
    % Convert coord pairs to arrays to make stack ops easier
    4 array astore /coord exch def % Shame!
    % Stack is currently [ x2 y2 x1 y1 ] (top)
    % X1 and Y1
    %aload 4 2 roll moveto
    %dup aload pop moveto
    % X1          Y1
    coord 2 get   coord 3 get moveto
    % X1          Y2
    coord 2 get   coord 1 get lineto
    % X2          Y2
    coord 0 get   coord 1 get lineto
    % X2          Y1
    coord 0 get   coord 3 get lineto
    closepath
  } def

  /strike
  {
    % Convert coord pairs to arrays to make stack ops easier
    4 array astore /coord exch def % Shame!
    % Stack is currently [ x2 y2 x1 y1 ] (top)
    % X1 and Y1
    %aload 4 2 roll moveto
    %dup aload pop moveto
    % X1          Y1
    coord 2 get   coord 3 get moveto
    % X2          Y2
    coord 0 get   coord 1 get lineto stroke
    % X1          Y2
    coord 2 get   coord 1 get moveto
    % X2          Y1
    coord 0 get   coord 3 get lineto stroke
  } def

  %%% Grid drawing
  %%  Usage: [ <x1> <y1> <x2> <y2> <box-length> <box-height> ] grid-draw
  /grid
  {
    /args exch def
    %% Vertical lines
    % Start               Inc            End
    args 0 get            args 4 get     args 2 get
    {
      dup
      % Y
      args 1 get moveto
      args 3 get  lineto
      stroke
    } for

    %% Horizontal lines
    % Start               Inc            End
    args 1 get            args 5 get     args 3 get
    {
      dup
      % X
      args 0 get exch moveto
      args 2 get exch lineto
      stroke
    } for
  } def

  /gridaddr
  {
    /args exch def
    args 5 get mul args 1 get add
    exch
    args 4 get mul args 0 get add
    exch
  } def

  /gridx1 1 in def
  /gridy1 height 1 in sub 1.3 in 5.6 mul sub def
  /gridx2 7.5 in def
  /gridy2 height 1 in sub 1.3 in 0.6 mul sub def
  /gridbw 1.3 in def
  /gridbh 1.3 in def
  /ga  { [ gridx1 gridy1 gridx2 gridy2 gridbw gridbh ] gridaddr } def
  /gat { ga 25 add exch 05 add exch } def

  /DeviceRGB setcolorspace
  $(float(col.r)) $(float(col.g)) $(float(col.b)) setcolor

  /URWGothic-Book 20 selectfont
  0 -0.25 ga moveto
  (Supporting) show
  /URWGothic-Demi 45 selectfont
  0 -0.65 ga moveto
  (El Cerrito) show
  0 -1.05 ga moveto
  (Forensics) show

  $(
  if draft
    """
    gsave
    0.8 setgray % Faint gray
    /Helvetica 175 selectfont
    2 2 ga moveto
    58 rotate
    (Draft) show
    grestore
    gsave
    0.0 setgray % Black
    20 setlinewidth
    0 0 ga 2.25 -1.05 ga strike
    grestore
    """
  else
    ""
  end
  )

  %1 in 1 in width 1 in sub height 1 in sub rect stroke

  $(float(col.r)) $(float(col.g)) $(float(col.b)) setcolor
  /Helvetica-Bold 12 selectfont
  3 -0.20 ga moveto
  (Name) show
  %3.50 -0.42 ga 5 -0.33 ga rect stroke
  3.50 -0.20 ga moveto 5 -0.20 ga lineto stroke

  3 -0.31 ga moveto
  (Card) show
  4 -0.31 ga moveto
  ($(@sprintf("%05u", num))) show

  3 -0.42 ga moveto
  (Round) show
  4 -0.42 ga moveto
  ($(@sprintf("%02u (%s)", round, col_name))) show

  $(handle_date(date))

  gridx1 gridy1 gridx2 gridy2 rect stroke
  [ gridx1 gridy1 gridx2 gridy2 gridbw gridbh ] grid

  /URWGothic-Book 46 selectfont
  2 2 ga moveto (Free) show

  /URWGothic-Book 72 selectfont
  0.2 5 ga moveto
  (BINGO)
  [1.2 gridbw mul
   0.8 gridbw mul
   0.9 gridbw mul
   1.0 gridbw mul
   1.0 gridbw mul]
  xshow

  /URWGothic-Book 72 selectfont
  $(mk_grid(ns)...)

  showpage
  """

end


"""
	MultipleCounts()
	MultipleCounts(counts, proba)

Holds something like the photon counting probabilities with their respective
probability (in order to use them as a single observation). Can be declared
empty as a placeholder.

	Fields:
		- counts::Union{Nothing, Vector{ModeOccupation}, Vector{PartitionOccupancy}},
		- proba::Union{Nothing,Vector{Real}}
"""
mutable struct MultipleCounts

	counts::Union{Nothing, Vector{ModeOccupation}, Vector{PartitionOccupancy}}
	proba::Union{Nothing,Vector{Real}}

	MultipleCounts() = new(nothing,nothing)
	MultipleCounts(counts, proba) = new(counts,proba)

end

"""
	EventProbability(probability::Union{Nothing, Number})
	EventProbability(mc::MultipleCounts)

Holds the probability or probabilities of an [`Event`](@ref).

	Fields:
		- probability::Union{Number,Nothing, MultipleCounts}
		- precision::Union{Number,Nothing}
		- failure_probability::Union{Number,Nothing}
"""
mutable struct EventProbability
    probability::Union{Number,Nothing, MultipleCounts}
    precision::Union{Number,Nothing} # see remarks in conventions
    failure_probability::Union{Number,Nothing}

    function EventProbability(probability::Union{Nothing, Number})

        if probability == nothing
            new(nothing, nothing, nothing)
        else
            try
                probability = clean_proba(probability)
                new(probability,nothing,nothing)
            catch
                error("invalid probability")
            end
        end
    end

	function EventProbability(mc::MultipleCounts)

		try
			mc.proba = clean_pdf(mc.proba)
			new(mc,nothing,nothing)
		catch
			error("invalid probability")
		end

	end

end

"""
	Event{TIn<:InputType, TOut<:OutputMeasurementType}

Event linking an input to an output.

	Fields:
		- input_state::Input{TIn}
		- output_measurement::TOut
		- proba_params::EventProbability
		- interferometer::Interferometer
"""
struct Event{TIn<:InputType, TOut<:OutputMeasurementType}

    input_state::Input{TIn}
    output_measurement::TOut
    proba_params::EventProbability
    interferometer::Interferometer

    function Event{TIn, TOut}(input_state, output_measurement, interferometer::Interferometer, proba_params = nothing) where {TIn<:InputType, TOut<:OutputMeasurementType}

        if proba_params == nothing
            proba_params = EventProbability(nothing)
        end

        new{TIn,TOut}(input_state, output_measurement, proba_params, interferometer)
    end

    Event(i,o,interf,p = nothing) = Event{get_parametric_type(i)[1], get_parametric_type(o)[1]}(i,o,interf,p)

end

function check_probability_empty(ev::Event)
    if ev.proba_params.probability != nothing
		@warn "probability was already set in, rewriting"
	end
end


"""
    update_wake_shedding_locations!(wakes, wake_shedding_locations,
        surfaces, ref, fs, dt, additional_velocity, Vte, nwake, eta)

Update the wake shedding locations.  Also update the first chordwise wake panels
to account for the new wake shedding location

# Arguments
 - `wakes`: Vector of wakes corresponding to each surface, represented by matrices
    of wake panels (see [`WakePanel`](@ref)) of shape (nw, ns) where `nw` is the
    number of chordwise wake panels and `ns` is the number of spanwise panels.
 - `wake_shedding_locations`: Shedding location coordinates for each surface for
    each trailing edge vertex.
 - `surfaces`: Vector of surfaces, represented by matrices of surface panels
    (see [`SurfacePanel`](@ref) of shape (nc, ns) where `nc` is the number of
    chordwise panels and `ns` is the number of spanwise panels
 - `reference`: Reference parameters (see [`Reference`](@ref))
 - `freestream`: Freestream parameters (see [`Freestream`](@ref))
 - `dt`: Time step (seconds)
 - `additional_velocity`: Function defining additional velocity field
 - `Vte`: Velocity experienced at the trailing edge due to surface motion.
 - `nwake`: Number of chordwise wake panels to use from each wake in `wakes`
 - `eta`: Time step fraction used to define separation between trailing
    edge and wake shedding location.  Typical values range from 0.2-0.3.
"""
@inline function update_wake_shedding_locations!(wakes, wake_shedding_locations,
    surfaces, ref, fs, dt, additional_velocity, Vte, nwake, eta)

    # get number of surfaces
    nsurf = length(surfaces)

    # loop through all surfaces
    for isurf = 1:nsurf

        # number of spanwise panels
        ns = size(wakes[isurf], 2)

        # update wake shedding location
        for j = 1:ns+1

            # extract trailing edge coordinate
            if j < ns + 1
                rte = bottom_left(surfaces[isurf][end, j])
            else
                rte = bottom_right(surfaces[isurf][end, j-1])
            end

            # freestream velocity
            V = freestream_velocity(fs)

            # rotational velocity
            V += rotational_velocity(rte, fs, ref)

            # additional velocity field
            if !isnothing(additional_velocity)
                V += additional_velocity(rte)
            end

            # velocity due to surface motion
            if !isnothing(Vte)
                V += Vte[isurf][j]
            end

            # update wake shedding location coordinates
            wake_shedding_locations[isurf][j] = rte + eta*V*dt

        end

        if nwake[isurf] > 0
            # loop through first row of wake panels
            for j = 1:ns
                # update wake panel with wake shedding location coordinates
                rtl = wake_shedding_locations[isurf][j]
                rtr = wake_shedding_locations[isurf][j+1]

                # preserve other wake panel coordinates
                rbl = bottom_left(wakes[isurf][1,j])
                rbr = bottom_right(wakes[isurf][1,j])

                # preserve core size
                core_size = get_core_size(wakes[isurf][1,j])

                # preserve circulation strength
                gamma = circulation_strength(wakes[isurf][1,j])

                # replace the old wake panel
                wakes[isurf][1,j] = WakePanel(rtl, rtr, rbl, rbr, core_size, gamma)
            end
        end

    end

    return wakes, wake_shedding_locations
end

"""
    get_wake_velocities!(wake_velocities, surfaces, wakes, ref, fs, Γ,
        additional_velocity, Vte, symmetric, repeated_points, nwake,
        surface_id, wake_finite_core, wake_shedding_locations, trailing_vortices, xhat)

# Arguments
 - `wake_velocities`: Velocities at the corners of the wake panels in `wakes`
 - `surfaces`: Vector of surfaces, represented by matrices of surface panels
    (see [`SurfacePanel`](@ref) of shape (nc, ns) where `nc` is the number of
    chordwise panels and `ns` is the number of spanwise panels
 - `wakes`: Vector of wakes corresponding to each surface, represented by matrices
    of wake panels (see [`WakePanel`](@ref)) of shape (nw, ns) where `nw` is the
    number of chordwise wake panels and `ns` is the number of spanwise panels.
 - `reference`: Reference parameters (see [`Reference`](@ref))
 - `freestream`: Freestream parameters (see [`Freestream`](@ref))
 - `Γ`: Circulation of all surface panels stored in a single vector
 - `additional_velocity`: Function defining additional velocity field
 - `Vte`: Velocity at the trailing edge vertices on each surface due to surface motion
 - `symmetric`: (required) Flag for each surface indicating whether a mirror
    image across the X-Z plane should be used when calculating induced velocities
 - `repeated_points`: Dictionary of the form `Dict((isurf, i) => [(jsurf1, j1),
    (jsurf2, j2)...]` which defines repeated trailing edge points.  Trailing edge
    point `i` on surface `isurf` is repeated on surface `jsurf1` at point `j1`,
    `jsurf2` at point `j2`, and so forth. See [`repeated_trailing_edge_points`](@ref)
 - `nwake`: Number of chordwise wake panels to use from each wake in `wakes`,
    defaults to all provided wake panels
 - `surface_id`: Surface ID for each surface.  The finite core model is disabled
    when calculating the influence of surfaces/wakes that share the same ID.
 - `wake_finite_core`: Flag for each wake indicating whether the finite core
    model should be enabled when calculating the wake's influence on itself and
    surfaces/wakes with the same surface ID.  Defaults to `true` for each surface.
 - `wake_shedding_locations`: Shedding location coordinates for each surface for
    each trailing edge vertex.
 - `trailing_vortices`: Flags to enable/disable trailing vortices, defaults to
    `true` for each surface
 - `xhat`: Direction in which to shed trailing vortices, defaults to [1, 0, 0]
"""
@inline function get_wake_velocities!(wake_velocities, surfaces, wakes, ref, fs, Γ,
    additional_velocity, Vte, symmetric, repeated_points, nwake,
    surface_id, wake_finite_core, wake_shedding_locations, trailing_vortices, xhat)

    # number of surfaces
    nsurf = length(surfaces)

    # loop through all surfaces
    for isurf = 1:nsurf

        # number of chordwise and spanwise panels
        nc, ns = size(surfaces[isurf])
        nw = nwake[isurf]

        # velocity at the wake shedding locations
        for is = 1:ns+1

            # check if this point is a duplicate, skip if it is
            if (isurf, is) in keys(repeated_points)
                for (jsurf, js) in repeated_points[(isurf, is)]
                    # NOTE: we assume that a point is not repeated on the same surface
                    if jsurf < isurf
                        wake_velocities[isurf][1, is] = wake_velocities[jsurf][1, js]
                        continue
                    end
                end
            end

            # get vertex location
            rc = wake_shedding_locations[isurf][is]

            # freestream velocity
            V = freestream_velocity(fs)

            # rotational velocity
            V += rotational_velocity(rc, fs, ref)

            # additional velocity field
            if !isnothing(additional_velocity)
                V += additional_velocity(rc)
            end

            # velocity at the trailing edge
            wake_velocities[isurf][1,is] = V
        end

        # velocity at all other wake vertices
        cr = CartesianIndices((2:nw+1, 1:ns+1))

        # loop through all vertices
        for I in cr

            # check if this point is a duplicate, skip if it is
            if (isurf, I[2]) in keys(repeated_points)
                for (jsurf, js) in repeated_points[(isurf, I[2])]
                    # NOTE: we assume that a point is not repeated on the same surface
                    if jsurf < isurf
                        wake_velocities[isurf][I] = wake_velocities[jsurf][I[1], js]
                        continue
                    end
                end
            end

            # get vertex location
            if I[1] <= nw && I[2] <= ns
                rc = top_left(wakes[isurf][I[1], I[2]])
            elseif I[1] == nw + 1 && I[2] <= ns
                rc = bottom_left(wakes[isurf][I[1]-1, I[2]])
            elseif I[1] <= nw && I[2] == ns + 1
                rc = top_right(wakes[isurf][I[1], I[2]-1])
            else # I[1] == nw + 1 && I[2] == ns + 1
                rc = bottom_right(wakes[isurf][I[1]-1, I[2]-1])
            end

            # freestream velocity
            wake_velocities[isurf][I] = freestream_velocity(fs)

            # rotational velocity
            wake_velocities[isurf][I] += rotational_velocity(rc, fs, ref)

            # additional velocity field
            if !isnothing(additional_velocity)
                wake_velocities[isurf][I] += additional_velocity(rc)
            end

            # induced velocity from each surface and wake
            jΓ = 0 # index for accessing Γ
            for jsurf = 1:nsurf

                # number of panels on sending surface
                Ns = length(surfaces[jsurf])

                # check if receiving point is repeated on the sending surface
                if isurf == jsurf
                    # the surfaces are the same
                    same_surface = true
                    # vertex spanwise coordinate
                    js = I[2]
                else
                    # the surfaces are different, but the point could still be a duplicate
                    if (isurf, I[2]) in keys(repeated_points)
                        # the vertex is duplicated

                        # check if the vertex is on the sending surface
                        idx = findfirst(x -> x[1] == jsurf, repeated_points[(isurf, I[2])])

                        if isnothing(idx)
                            # the vertex is not duplicated on the sending surface
                            same_surface = false
                        else
                            # the vertex is duplicated on the sending surface
                            same_surface = true
                            # vertex spanwise coordinate on sending surface
                            js = repeated_points[(isurf, I[2])][idx][2]
                        end
                    else
                        # the vertex is not duplicated

                        # the vertex is not on the sending surface
                        same_surface = false
                    end
                end

                # extract circulation values corresponding to the sending surface
                vΓ = view(Γ, jΓ+1:jΓ+Ns)

                # induced velocity from this surface
                wake_velocities[isurf][I] += induced_velocity(rc, surfaces[jsurf], vΓ;
                    finite_core = surface_id[isurf] != surface_id[jsurf],
                    wake_shedding_locations = wake_shedding_locations[jsurf],
                    symmetric = symmetric[jsurf],
                    trailing_vortices = false,
                    xhat = xhat)

                # add induced velocity from the wake
                if same_surface
                    # vertex location on wake
                    J = CartesianIndex(I[1], js)

                    # induced velocity from wake on its own vertex
                    wake_velocities[isurf][I] += induced_velocity(J, wakes[jsurf];
                        finite_core = wake_finite_core[jsurf] || surface_id[isurf] != surface_id[jsurf],
                        symmetric = symmetric[jsurf],
                        nc = nwake[jsurf],
                        trailing_vortices = trailing_vortices[jsurf],
                        xhat = xhat)
                else
                    # induced velocity from wake on another wake's vertex
                    wake_velocities[isurf][I] += induced_velocity(rc, wakes[jsurf];
                        finite_core = wake_finite_core[jsurf] || surface_id[isurf] != surface_id[jsurf],
                        symmetric = symmetric[jsurf],
                        nc = nwake[jsurf],
                        trailing_vortices = trailing_vortices[jsurf],
                        xhat = xhat)
                end

                jΓ += Ns # increment Γ index
            end
        end
    end

    return wake_velocities
end

"""
    translate_wake(panel, wake_velocities, dt)

Return a translated copy of the wake panel `panel` given the wake corner velocities
`wake_velocities` and the time step `dt`

# Arguments
 - `panel`: Wake panel (of type [`WakePanel`](@ref))
 - `wake_velocities`: Matrix containing the velocities at each of the four corners
    of `panel`
 - `dt`: Time step (seconds)
"""
@inline function translate_wake(panel::WakePanel, wake_velocities, dt)

    # extract corners
    rtl = top_left(panel)
    rtr = top_right(panel)
    rbl = bottom_left(panel)
    rbr = bottom_right(panel)

    # get vortex filament length
    lt = norm(rtr - rtl)
    lb = norm(rbl - rbr)
    ll = norm(rtl - rbl)
    lr = norm(rbr - rtr)
    l1 = lt + lb + ll + lr

    # translate corners
    rtl += wake_velocities[1,1]*dt
    rtr += wake_velocities[1,2]*dt
    rbl += wake_velocities[2,1]*dt
    rbr += wake_velocities[2,2]*dt

    # get new vortex filament length
    lt = norm(rtr - rtl)
    lb = norm(rbl - rbr)
    ll = norm(rtl - rbl)
    lr = norm(rbr - rtr)
    l2 = lt + lb + ll + lr

    # use previous core size
    core_size = get_core_size(panel)

    # correct vorticity for vortex stretching
    gamma = circulation_strength(panel)*l1/l2

    return WakePanel(rtl, rtr, rbl, rbr, core_size, gamma)
end

"""
    translate_wake!(wake, wake_velocities, dt; nwake = size(wake, 1))

Translate the wake panels in `wake` given the corner velocities `wake_velocities`
and the time step `dt`.

# Arguments
 - `wake`: Matrix of wake panels (see [`WakePanel`](@ref)) of shape (nw, ns)
    where `nw` is the number of chordwise wake panels and `ns` is the number of
    spanwise panels, defaults to no wake panels
 - `wake_velocities`: Velocities at each of the vertices corresponding to the
    wake panels in `wake`
 - `dt`: Time step

# Keyword Arguments
 - `nwake`: Number of chordwise wake panels to use from `wake`, defaults to all
    provided wake panels
"""
@inline function translate_wake!(wake, wake_velocities, dt; nwake = size(wake, 1))

    nw = nwake
    ns = size(wake, 2)
    cw = CartesianIndices((nw, ns))

    for I in cw

        panel = wake[I]

        vV = view(wake_velocities, I[1]:I[1]+1, I[2]:I[2]+1)

        wake[I] = translate_wake(panel, vV, dt)
    end

    return wake
end

"""
    shed_wake!(wake, wake_shedding_locations, wake_velocities, dt, surface, Γ, nwake)

Shed a new wake panel from the wake shedding locations and translate existing
wake panels.

# Arguments
 - `wake`: Matrix of wake panels (see [`WakePanel`](@ref)) of shape (nw, ns)
    where `nw` is the number of chordwise wake panels and `ns` is the number of
    spanwise panels
 - `wake_shedding_locations`: Vector of length `ns` which stores the coordinates
    where wake panels are shed from the trailing edge of `surface`.
 - `wake_velocities`: Velocities at each of the vertices corresponding to the
    wake panels in `wake`
 - `dt`: Time step (seconds)
 - `surface`: Matrix of surface panels (see [`SurfacePanel`](@ref)) of shape
    (nc, ns) where `nc` is the number of chordwise panels and `ns` is the number
    of spanwise panels
 - `Γ`: Circulation strength of each surface panel in `surface`
 - `nwake`: Number of chordwise wake panels to use from `wake`, defaults to all
    provided wake panels
"""
@inline function shed_wake!(wake::AbstractMatrix, wake_shedding_locations,
    wake_velocities, dt, surface, Γ, nwake)

    nc, ns = size(surface)
    nw = size(wake, 1)
    ls = LinearIndices((nc, ns))

    # replace the last chordwise panels with the newly shed wake panels
    for j = 1:ns

        # shedding location coordinates
        rtl = wake_shedding_locations[j]
        rtr = wake_shedding_locations[j+1]

        # shed coordinates
        rbl = rtl + wake_velocities[1, j]*dt
        rbr = rtr + wake_velocities[1, j+1]*dt

        # use core size from the shedding panel
        core_size = get_core_size(surface[end, j])

        # use circulation strength from the shedding panel
        gamma = Γ[ls[end,j]]

        # replace the oldest wake panel
        wake[end,j] = WakePanel(rtl, rtr, rbl, rbr, core_size, gamma)
    end

    # translate all existing wake panels except the most recently shed panels
    translate_wake!(wake, wake_velocities, dt, nwake = min(nwake, nw-1))

    # shift wake panels to make newly shed wake panel first
    rowshift!(wake)

    return wake
end

"""
    shed_wake!(wakes, wake_shedding_locations, wake_velocities, dt, surfaces, Γ, nwake)

Shed a new wake panel from the wake shedding locations and translate existing
wake panels.

# Arguments
 - `wakes`: Vector of wakes corresponding to each surface, represented by matrices
    of wake panels (see [`WakePanel`](@ref)) of shape (nw, ns) where `nw` is the
    number of chordwise wake panels and `ns` is the number of spanwise panels.
 - `wake_shedding_locations`: Shedding location coordinates for each surface for
    each trailing edge vertex.
 - `wake_velocities`: Velocities at each of the vertices corresponding to the
    wake panels in `wake`
 - `dt`: Time step (seconds)
 - `surfaces`: Vector of surfaces, represented by matrices of surface panels
    (see [`SurfacePanel`](@ref) of shape (nc, ns) where `nc` is the number of
    chordwise panels and `ns` is the number of spanwise panels
 - `Γ`: Circulation strength of each surface panel in `surfaces`
 - `nwake`: Number of chordwise wake panels to use from each wake in `wakes`,
    defaults to all provided wake panels
"""
@inline function shed_wake!(wakes::AbstractVector{<:AbstractMatrix}, wake_shedding_locations,
    wake_velocities, dt, surfaces, Γ, nwake)

    iΓ = 0
    for i = 1:length(surfaces)

        N = length(surfaces[i])

        vΓ = view(Γ, iΓ+1:iΓ+N)

        shed_wake!(wakes[i], wake_shedding_locations[i], wake_velocities[i], dt,
            surfaces[i], vΓ, nwake[i])

        iΓ += N
    end

    return wakes
end

"""
    rowshift!(A)

Circularly shifts the rows of a matrix down one row.
"""
function rowshift!(A)

    ni, nj = size(A)

    for j = 1:nj
        tmp = A[ni,j]
        for i = ni:-1:2
            A[i,j] = A[i-1,j]
        end
        A[1,j] = tmp
    end

    return A
end


# Copyright (c) 2018: Matthew Wilhelm & Matthew Stuber.
# This code is licensed under MIT license (see LICENSE.md for full details)
#############################################################################
# EAGO
# A development environment for robust and global optimization
# See https://github.com/PSORLab/EAGO.jl
#############################################################################
# src/eago_optimizer/optimize/optimize_convex.jl
# Contains the solve_local_nlp! routine which computes the optimal value
# of a convex function. This is used to compute the upper bound in the
# branch and bound routine. A number of utility function required for
# solve_local_nlp! are also included.
#############################################################################

"""

Shifts the resulting local nlp objective value `f*` by `(1.0 + relative_tolerance/100.0)*f* + absolute_tolerance/100.0`.
This assumes that the local solvers relative tolerance and absolute tolerance is significantly lower than the global
tolerance (local problem is minimum).
"""
function stored_adjusted_upper_bound!(d::GlobalOptimizer, v::Float64)
    adj_atol = d._parameters.absolute_tolerance/100.0
    adj_rtol = d._parameters.relative_tolerance/100.0
    if v > 0.0
        d._upper_objective_value = v*(1.0 + adj_rtol) + adj_atol
    else
        d._upper_objective_value = v*(1.0 - adj_rtol) + adj_atol
    end
    return nothing
end

function _update_upper_variables!(d, m::GlobalOptimizer{R,S,Q}) where {R,S,Q<:ExtensionType}
    for i = 1:_variable_num(FullVar(), m)
        v = m._upper_variables[i]
        l  = _lower_bound(FullVar(), m, i)
        u  = _upper_bound(FullVar(), m, i)
        if is_integer(FullVar(), m, i)
            l = ceil(l)
            u = floor(u)
        end
        is_fixed_int = l == u
        vi = _working_variable_info(m,i)
        if is_fixed(vi) || is_fixed_int
            MOI.add_constraint(d, v, ET(l))
        elseif is_less_than(vi)
            MOI.add_constraint(d, v, LT(u))
        elseif is_greater_than(vi)
            MOI.add_constraint(d, v, GT(l))
        elseif is_real_interval(vi)
            MOI.add_constraint(d, v, LT(u))
            MOI.add_constraint(d, v, GT(l))
        end
    end
    return
end

function _finite_mid(l::T, u::T) where T
    (isfinite(l) && isfinite(u)) && return 0.5*(l + u)
    isfinite(l) ? l : (isfinite(u) ? u : zero(T))
end
function _set_starting_point!(d, m::GlobalOptimizer{R,S,Q}) where {R,S,Q<:ExtensionType}
    for i = 1:_variable_num(FullVar(), m)
        l  = _lower_bound(FullVar(), m, i)
        u  = _upper_bound(FullVar(), m, i)
        v = m._upper_variables[i]
        MOI.set(d, MOI.VariablePrimalStart(), v, _finite_mid(l, u))
    end
    return
end

"""
    LocalResultStatus

Status code used internally to determine how to interpret the results from the
solution of a local problem solve.
"""
@enum(LocalResultStatus, LRS_FEASIBLE, LRS_OTHER)

"""
$(SIGNATURES)

Takes an `MOI.TerminationStatusCode` and a `MOI.ResultStatusCode` and returns `true`
if this corresponds to a solution that is proven to be feasible.
Returns `false` otherwise.
"""
function local_problem_status(t::MOI.TerminationStatusCode, r::MOI.ResultStatusCode)

    if (t == MOI.OPTIMAL) && (r == MOI.FEASIBLE_POINT)
        return LRS_FEASIBLE
    elseif (t == MOI.LOCALLY_SOLVED) && (r == MOI.FEASIBLE_POINT)
        return LRS_FEASIBLE
    # This is default solver specific... the acceptable constraint tolerances
    # are set to the same values as the basic tolerance. As a result, an
    # acceptably solved solution is feasible but non necessarily optimal
    # so it should be treated as a feasible point
    elseif (t == MOI.ALMOST_LOCALLY_SOLVED) && (r == MOI.NEARLY_FEASIBLE_POINT)
        return LRS_FEASIBLE
    end
    return LRS_OTHER
end

function _unpack_local_nlp_solve!(m::GlobalOptimizer, d::T) where T

    tstatus = MOI.get(d, MOI.TerminationStatus())
    pstatus = MOI.get(d, MOI.PrimalStatus())
    m._upper_termination_status = tstatus
    m._upper_result_status = pstatus

    if local_problem_status(tstatus, pstatus) == LRS_FEASIBLE
        if is_integer_feasible(m)

            m._upper_feasibility = true
            obj_val = MOI.get(d, MOI.ObjectiveValue())
            stored_adjusted_upper_bound!(m, obj_val)
            m._best_upper_value = min(obj_val, m._best_upper_value)
            m._upper_solution .= MOI.get(d, MOI.VariablePrimal(), m._upper_variables)

            ip = m._input_problem
            _extract_primal_linear!(d, ip)
            _extract_primal_quadratic!(d, ip)
        end
    else
        m._upper_feasibility = false
        m._upper_objective_value = Inf
    end
    return
end

"""

Constructs and solves the problem locally on on node `y` updated the upper
solution informaton in the optimizer.
"""
function solve_local_nlp!(m::GlobalOptimizer{R,S,Q}) where {R,S,Q<:ExtensionType}

    upper_optimizer = _upper_optimizer(m)
    MOI.empty!(upper_optimizer)

    for i = 1:m._working_problem._variable_count
        m._upper_variables[i] = MOI.add_variable(upper_optimizer)
    end
    _update_upper_variables!(upper_optimizer, m)
    _set_starting_point!(upper_optimizer, m)

    # add constraints
    ip = m._input_problem
    _add_constraint_store_ci_linear!(upper_optimizer, ip)
    _add_constraint_store_ci_quadratic!(upper_optimizer, ip)
     #add_soc_constraints!(m, upper_optimizer)

    # Add nonlinear evaluation block
    MOI.set(upper_optimizer, MOI.NLPBlock(), m._working_problem._nlp_data)
    MOI.set(upper_optimizer, MOI.ObjectiveSense(), MOI.MIN_SENSE)
    MOI.set(upper_optimizer, MOI.ObjectiveFunction{SAF}(), m._working_problem._objective_saf)

    # Optimizes the object
    MOI.optimize!(upper_optimizer)

    #println(" ")
    #@show m._working_problem._objective_saf
    #@show upper_optimizer.optimizer.model.quadratic_le_constraints[1]
    #@show typeof(upper_optimizer.optimizer.model.quadratic_le_constraints[1])
    #@show upper_optimizer.optimizer.model.quadratic_ge_constraints
    #@show upper_optimizer.optimizer.model.quadratic_eq_constraints

    #@show upper_optimizer.optimizer.model.nlp_data
    #@show fieldnames(typeof(upper_optimizer.optimizer.model.nlp_data))

    _unpack_local_nlp_solve!(m, upper_optimizer)
end

function optimize!(::DIFF_CVX, m::GlobalOptimizer)
    solve_local_nlp!(m)
    return
end


module MessageTypes

using Aerospace

export
Health,
CoopFormation,
CoopProNav

type Health
    time::Float64
    icom::Bool
    igps::Bool
    num_sats::UInt8
    TAS::Float32
    IAS::Float32
    alt_hae::Float64
end

type CoopFormation
    time::Float64
    UTCtime::Float64
    rm_ecef::Array{Float64,1}
    vm_ecef::Array{Float64,1}
    rm_lead_ecef::Array{Float64,1}
    vm_lead_ecef::Array{Float64,1}
end

type CoopProNav
    time::Float64
    UTCtime::Float64
    rm_ecef::Array{Float64,1}
    vm_ecef::Array{Float64,1}
    rt_ecef::Array{Float64,1}
    vt_ecef::Array{Float64,1}
    tgo::Float64
end

end


using Distributed, Dates
t1 = now()
addprocs()

@everywhere begin

    using Dates, Glob, SeisIO, SeisNoise, Serialization

    # directories
    BASEDIR = "/media/four/"
    DATADIR = joinpath(BASEDIR,"LASSO/SAC/")
    FFTDIR = joinpath(BASEDIR,"LASSO/FFT/")
    XMLDIR = joinpath(BASEDIR,"LASSO/XML")
    files = glob("*.SAC",DATADIR)

    if !isdir(FFTDIR)
        mkpath(FFTDIR)
    end

    # parameters
    fs = 250.
    cc_len = 900.
    cc_step = 450.
    freqmin = 0.1
    freqmax = 20.

    # fast saving of FFT
    function serialize_fft(F::FFTData,FFTDIR::String)
        filename = joinpath(FFTDIR,F.name)
        if isa(F.fft,SeisNoise.AbstractGPUArray)
            F = F |> cpu
        end
        serialize(filename,F)
        return nothing
    end

    function seis2fft(
        file::String,
        XMLDIR::String,
        FFTDIR::String,
        fs::Real,
        cc_len::Real,
        cc_step::Real,
        freqmin::Float64,
        freqmax::Float64,
    )
        println("Reading file $(basename(file))")
        S = read_data("sac",file)
        S.fs .= round.(S.fs)
        detrend!(S)
        taper!(S)
        ungap!(S)
        filtfilt!(S,rt="Lowpass",fh=fs/2,np=3)
        resample!(S,fs=fs)

        # get xmlfile
        net,sta,loc,chan = split(S[1].id,'.')
        xmlfile = joinpath(XMLDIR,net*'.'*sta*".xml")
        Resp = read_meta("sxml",xmlfile)
        S.loc[1] = Resp.loc[1]
        translate_resp!(S,Resp[1].resp)
        remove_resp!(S)
        filtfilt!(S,fl=freqmin,fh=freqmax,np=2)
        R = RawData(S[1],cc_len,cc_step)
        demean!(R)
        taper!(R)
        F = rfft(R)
        serialize_fft(F,FFTDIR)
        return nothing
    end
end

t2 = now()
pmap((file -> seis2fft(file,XMLDIR,FFTDIR,fs,cc_len,cc_step,freqmin,freqmax)),files)
t3 = now()

# time for stuff
compute = Dates.canonicalize(Dates.CompoundPeriod(t3-t2))
total = Dates.canonicalize(Dates.CompoundPeriod(t3-t1))
println("Total computation took $total")
println("Computation took $compute")


abstract type DescentDirectionAllocs{T <: Real, S} end

mutable struct DescentDirectionAllocsPC{T <: Real, S} <: DescentDirectionAllocs{T, S}
  Δxy_aff::S # affine-step solution of the augmented system [Δx_aff; Δy_aff], size nvar + ncon
  Δs_l_aff::S # size nlow
  Δs_u_aff::S # size nupp
  x_m_l_αΔ_aff::S # x + α_aff * Δxy_aff - lvar , size nlow
  u_m_x_αΔ_aff::S # uvar - (x + α_aff * Δxy_aff) , size nupp
  s_l_αΔ_aff::S # s_l + α_aff * Δs_l_aff , size nlow
  s_u_αΔ_aff::S # s_u + α_aff * Δs_u_aff , size nupp
  rxs_l::S # - σ * μ * e + ΔX_aff * Δ_S_l_aff , size nlow
  rxs_u::S # σ * μ * e + ΔX_aff * Δ_S_u_aff , size nupp
  function DescentDirectionAllocsPC(
    Δxy_aff::AbstractVector{T},
    Δs_l_aff::AbstractVector{T},
    Δs_u_aff::AbstractVector{T},
    x_m_l_αΔ_aff::AbstractVector{T},
    u_m_x_αΔ_aff::AbstractVector{T},
    s_l_αΔ_aff::AbstractVector{T},
    s_u_αΔ_aff::AbstractVector{T},
    rxs_l::AbstractVector{T},
    rxs_u::AbstractVector{T},
  ) where {T <: Real}
    S = typeof(Δxy_aff)
    return new{T, S}(
      Δxy_aff,
      Δs_l_aff,
      Δs_u_aff,
      x_m_l_αΔ_aff,
      u_m_x_αΔ_aff,
      s_l_αΔ_aff,
      s_u_αΔ_aff,
      rxs_l,
      rxs_u,
    )
  end
end

DescentDirectionAllocsPC(id::QM_IntData, S::DataType) where {T <: Real} = DescentDirectionAllocsPC(
  S(undef, id.nvar + id.ncon), # Δxy_aff
  S(undef, id.nlow), # Δs_l_aff
  S(undef, id.nupp), # Δs_u_aff
  S(undef, id.nlow), # x_m_l_αΔ_aff
  S(undef, id.nupp), # u_m_x_αΔ_aff
  S(undef, id.nlow), # s_l_αΔ_aff
  S(undef, id.nupp), # s_u_αΔ_aff
  S(undef, id.nlow), # rxs_l
  S(undef, id.nupp),  # rxs_u
)

convert(
  ::Type{<:DescentDirectionAllocs{T, S}},
  dda::DescentDirectionAllocsPC{T0, S0},
) where {T <: Real, S, T0 <: Real, S0} = DescentDirectionAllocsPC(
  convert(S.name.wrapper{T, 1}, dda.Δxy_aff),
  convert(S.name.wrapper{T, 1}, dda.Δs_l_aff),
  convert(S.name.wrapper{T, 1}, dda.Δs_u_aff),
  convert(S.name.wrapper{T, 1}, dda.x_m_l_αΔ_aff),
  convert(S.name.wrapper{T, 1}, dda.u_m_x_αΔ_aff),
  convert(S.name.wrapper{T, 1}, dda.s_l_αΔ_aff),
  convert(S.name.wrapper{T, 1}, dda.s_u_αΔ_aff),
  convert(S.name.wrapper{T, 1}, dda.rxs_l),
  convert(S.name.wrapper{T, 1}, dda.rxs_u),
)

function update_pt_aff!(
  x_m_l_αΔ_aff,
  u_m_x_αΔ_aff,
  s_l_αΔ_aff,
  s_u_αΔ_aff,
  Δxy_aff,
  Δs_l_aff,
  Δs_u_aff,
  x_m_lvar,
  uvar_m_x,
  s_l,
  s_u,
  α_aff_pri,
  α_aff_dual,
  ilow,
  iupp,
)
  x_m_l_αΔ_aff .= @views x_m_lvar .+ α_aff_pri .* Δxy_aff[ilow]
  u_m_x_αΔ_aff .= @views uvar_m_x .- α_aff_pri .* Δxy_aff[iupp]
  s_l_αΔ_aff .= s_l .+ α_aff_dual .* Δs_l_aff
  s_u_αΔ_aff .= s_u .+ α_aff_dual .* Δs_u_aff
end

# Mehrotra's Predictor-Corrector algorithm
function update_dd!(
  dda::DescentDirectionAllocsPC{T},
  pt::Point{T},
  itd::IterData{T},
  fd::Abstract_QM_FloatData{T},
  id::QM_IntData,
  res::AbstractResiduals{T},
  pad::PreallocatedData{T},
  cnts::Counters,
  T0::DataType,
) where {T <: Real}

  # solve system aff
  dda.Δxy_aff[1:(id.nvar)] .= .-res.rc
  dda.Δxy_aff[(id.nvar + 1):(id.nvar + id.ncon)] .= .-res.rb
  if typeof(pad) <: PreallocatedDataAugmented || typeof(pad) <: PreallocatedDataNormal
    dda.Δxy_aff[id.ilow] .+= pt.s_l
    dda.Δxy_aff[id.iupp] .-= pt.s_u
  elseif typeof(pad) <: PreallocatedDataNewton
    dda.Δs_l_aff .= .-itd.x_m_lvar .* pt.s_l
    dda.Δs_u_aff .= .-itd.uvar_m_x .* pt.s_u
  end

  cnts.w.write == true && write_system(cnts.w, pad.K, dda.Δxy_aff, :aff, cnts.k)
  out = solver!(dda.Δxy_aff, pad, dda, pt, itd, fd, id, res, cnts, T0, :aff)
  out == 1 && return out
  if typeof(pad) <: PreallocatedDataAugmented || typeof(pad) <: PreallocatedDataNormal
    dda.Δs_l_aff .= @views .-pt.s_l .- pt.s_l .* dda.Δxy_aff[id.ilow] ./ itd.x_m_lvar
    dda.Δs_u_aff .= @views .-pt.s_u .+ pt.s_u .* dda.Δxy_aff[id.iupp] ./ itd.uvar_m_x
  end

  if typeof(pt.x) <: Vector
    α_aff_pri, α_aff_dual = compute_αs(
      pt.x,
      pt.s_l,
      pt.s_u,
      fd.lvar,
      fd.uvar,
      dda.Δxy_aff,
      dda.Δs_l_aff,
      dda.Δs_u_aff,
      id.nvar,
    )
  else
    α_aff_pri, α_aff_dual = compute_αs_gpu(
      pt.x,
      pt.s_l,
      pt.s_u,
      fd.lvar,
      fd.uvar,
      dda.Δxy_aff,
      dda.Δs_l_aff,
      dda.Δs_u_aff,
      id.nvar,
      itd.store_vpri,
      itd.store_vdual_l,
      itd.store_vdual_u,
    )
  end

  # (x-lvar, uvar-x, s_l, s_u) .+= α_aff * Δ_aff
  update_pt_aff!(
    dda.x_m_l_αΔ_aff,
    dda.u_m_x_αΔ_aff,
    dda.s_l_αΔ_aff,
    dda.s_u_αΔ_aff,
    dda.Δxy_aff,
    dda.Δs_l_aff,
    dda.Δs_u_aff,
    itd.x_m_lvar,
    itd.uvar_m_x,
    pt.s_l,
    pt.s_u,
    α_aff_pri,
    α_aff_dual,
    id.ilow,
    id.iupp,
  )

  μ_aff =
    compute_μ(dda.x_m_l_αΔ_aff, dda.u_m_x_αΔ_aff, dda.s_l_αΔ_aff, dda.s_u_αΔ_aff, id.nlow, id.nupp)
  σ = (μ_aff / itd.μ)^3

  # corrector-centering step
  if typeof(pad) <: PreallocatedDataAugmented || typeof(pad) <: PreallocatedDataNormal
    dda.rxs_l .= @views -σ * itd.μ .+ dda.Δxy_aff[id.ilow] .* dda.Δs_l_aff
    dda.rxs_u .= @views σ * itd.μ .+ dda.Δxy_aff[id.iupp] .* dda.Δs_u_aff
    itd.Δxy .= 0
    itd.Δxy[id.ilow] .+= dda.rxs_l ./ itd.x_m_lvar
    itd.Δxy[id.iupp] .+= dda.rxs_u ./ itd.uvar_m_x
  elseif typeof(pad) <: PreallocatedDataNewton
    itd.Δxy[1:end] .= 0
    itd.Δs_l .= @views σ * itd.μ .- dda.Δxy_aff[id.ilow] .* dda.Δs_l_aff
    itd.Δs_u .= @views σ * itd.μ .+ dda.Δxy_aff[id.iupp] .* dda.Δs_u_aff
  end

  cnts.w.write == true && write_system(cnts.w, pad.K, itd.Δxy, :cc, cnts.k)
  out = solver!(itd.Δxy, pad, dda, pt, itd, fd, id, res, cnts, T0, :cc)
  out == 1 && return out
  if typeof(pad) <: PreallocatedDataAugmented || typeof(pad) <: PreallocatedDataNormal
    itd.Δs_l .= @views .-(dda.rxs_l .+ pt.s_l .* itd.Δxy[id.ilow]) ./ itd.x_m_lvar
    itd.Δs_u .= @views (dda.rxs_u .+ pt.s_u .* itd.Δxy[id.iupp]) ./ itd.uvar_m_x
  end

  # final direction
  itd.Δxy .+= dda.Δxy_aff
  itd.Δs_l .+= dda.Δs_l_aff
  itd.Δs_u .+= dda.Δs_u_aff

  return out
end

mutable struct DescentDirectionAllocsIPF{T <: Real, S} <: DescentDirectionAllocs{T, S}
  compl::S # complementarity s_lᵀ(x-lvar) + s_uᵀ(uvar-x)
  function DescentDirectionAllocsIPF(compl::AbstractVector{T}) where {T <: Real}
    S = typeof(compl)
    return new{T, S}(compl)
  end
end

DescentDirectionAllocsIPF(id::QM_IntData, S::DataType) =
  DescentDirectionAllocsIPF(S(undef, id.nvar))

convert(
  ::Type{<:DescentDirectionAllocs{T, S}},
  dda::DescentDirectionAllocsIPF{T0, S0},
) where {T <: Real, S, T0 <: Real, S0} =
  DescentDirectionAllocsIPF(convert(S.name.wrapper{T, 1}, dda.compl))

function update_dd!(
  dda::DescentDirectionAllocsIPF{T},
  pt::Point{T},
  itd::IterData{T},
  fd::Abstract_QM_FloatData{T},
  id::QM_IntData,
  res::AbstractResiduals{T},
  pad::PreallocatedData{T},
  cnts::Counters,
  T0::DataType,
) where {T <: Real}
  r, γ = T(0.999), T(0.05)
  # D = [s_l (x-lvar) + s_u (uvar-x)]
  dda.compl .= 0
  dda.compl[id.ilow] .+= pt.s_l .* itd.x_m_lvar
  dda.compl[id.iupp] .+= pt.s_u .* itd.uvar_m_x
  dda.compl[id.ifree] .= one(T)
  ξ = minimum(dda.compl) / itd.μ
  σ = γ * min((one(T) - r) * (one(T) - ξ) / ξ, T(2))^3

  itd.Δxy[1:(id.nvar)] .= .-res.rc
  itd.Δxy[(id.nvar + 1):(id.nvar + id.ncon)] .= .-res.rb
  if typeof(pad) <: PreallocatedDataAugmented || typeof(pad) <: PreallocatedDataNormal
    itd.Δxy[id.ilow] .+= pt.s_l - σ * itd.μ ./ itd.x_m_lvar
    itd.Δxy[id.iupp] .-= pt.s_u - σ * itd.μ ./ itd.uvar_m_x
  elseif typeof(pad) <: PreallocatedDataNewton
    itd.Δs_l .= σ * itd.μ .- itd.x_m_lvar .* pt.s_l
    itd.Δs_u .= σ * itd.μ .- itd.uvar_m_x .* pt.s_u
  end

  cnts.w.write == true && write_system(cnts.w, pad.K, itd.Δxy, :IPF, cnts.k)
  out = solver!(itd.Δxy, pad, dda, pt, itd, fd, id, res, cnts, T0, :IPF)
  out == 1 && return out
  if typeof(pad) <: PreallocatedDataAugmented || typeof(pad) <: PreallocatedDataNormal
    itd.Δs_l .= @views (σ * itd.μ .- pt.s_l .* itd.Δxy[id.ilow]) ./ itd.x_m_lvar .- pt.s_l
    itd.Δs_u .= @views (σ * itd.μ .+ pt.s_u .* itd.Δxy[id.iupp]) ./ itd.uvar_m_x .- pt.s_u
  end
end