using MetidaBase
using Test, Tables, TypedTables, CSV

@testset "MetidaBase.jl" begin
    io       = IOBuffer();
    # Metida table names - auto
    mt = MetidaBase.metida_table([1,2,3], ["a", "b", "c"])
    @test names(mt) == [:x1, :x2]
    # Metida table names - defined
    mt = MetidaBase.metida_table([1,2,3], ["a", "b", "c"], names = (:a, :b))
    # Push row
    pushfirst!(mt, [0, " "])
    @test mt[1, :a] == 0
    # Set element
    mt[1, :b] = "_"
    @test mt[1, :b] == "_"
    # push Named tuple as row
    ntr = NamedTuple{(:b, :a)}(["d", 10])
    pushfirst!(mt, ntr)
    @test mt[1, :b] == "d"
    # Test
    @test mt[:, :b] == ["d", "_", "a", "b", "c"]
    # Test show
    @test_nowarn Base.show(io, mt)

    # Tst size
    @test size(mt, 1) == 5
    @test size(mt, 2) == 2

    # Tables rows method
    rows = Tables.rows(mt)

    # MetidaTable > TypedTables
    df = Table(mt)

    # Enumerate
    for (i,j) in enumerate(mt)
        @test mt[i, :a] == j[1]
        @test mt[i, :b] == j[2]
    end

    # Appent one table to another
    l1 = length(mt)
    mt2 = MetidaBase.metida_table([1,2,3], ["a", "b", "c"], names = (:a, :b))
    append!(mt, mt2)
    @test l1 + length(mt2) == length(mt)
    mt2 = MetidaBase.metida_table(["e", "f", "g"], [1,2,3], names = (:b, :a))
    append!(mt, mt2)

    # CSV compat test
    @test_nowarn CSV.write(io, mt)

    # Structures
    ############################################################################
    struct ExampleIDStruct <: MetidaBase.AbstractSubject
        #time
        #obs
        id::Dict
    end

    struct ExampleResultStruct{T} <: MetidaBase.AbstractSubjectResult{T}
        data::T
        result::Dict
    end
    exiddsv = Vector{ExampleIDStruct}(undef, 3)
    for i in 1:3
        exiddsv[i] = ExampleIDStruct(Dict(:a => 1, :b => 1))
    end
    exidds = MetidaBase.DataSet(exiddsv)
    ############################################################################
    # DATASET Tests
    # Test length
    @test length(MetidaBase.getdata(exidds)) == length(exidds)
    # getid
    MetidaBase.getid(exidds[2])[:a] = 3
    MetidaBase.getid(exidds[3])[:a] = 2
    MetidaBase.getid(exidds[2])[:b] = 2
    MetidaBase.getid(exidds[3])[:b] = 3
    MetidaBase.getid(exidds, :, :a)

    exrsdsv = Vector{ExampleResultStruct}(undef, length(exidds))
    for i in 1:length(exidds)
        exrsdsv[i] = ExampleResultStruct(exidds[i], Dict(:r1 => 3, :r2 => 4))
    end
    exrsds = MetidaBase.DataSet(exrsdsv)

    @test exrsds[:, :r1][1] == 3
    @test exrsds[1, :r1] == 3

    @test MetidaBase.getid(exidds[3], :a) == 2
    sort!(exidds, :a)
    @test MetidaBase.getid(exidds[3], :a) == 3
    MetidaBase.getid(exrsds, :, :a)

    @test_nowarn sort!(exrsds, :a)

    @test first(exrsds) == exrsds[1]

    MetidaBase.uniqueidlist(exidds, [:a])
    MetidaBase.uniqueidlist(exidds, :a)

    MetidaBase.subset(exidds, Dict(:a => 1))
    MetidaBase.subset(exrsds, Dict(:a => 1))
    MetidaBase.subset(exrsds, 1:2)

    @test_nowarn map(identity, exidds)

    filtexrsds = filter(x -> x.id[:a] == 2, exidds)
    filter!(x -> x.id[:a] == 2, exidds)

    @test length(filtexrsds) == length(exidds)
    @test filtexrsds[1].id[:a] == exidds[1].id[:a] == 2

    mt  = MetidaBase.metida_table(exrsds)
    mt  = MetidaBase.metida_table(exrsds; results = :r1, ids = :a)
    smt = MetidaBase.Tables.schema(mt)
    # TypedTables export
    @test_nowarn Table(exrsds; results = :r1, ids = [:a, :b])
    #Iterators data
    v1 = [1,2,-6,missing,NaN]
    #Iterators tests
    itr1 = MetidaBase.skipnanormissing(v1)
    for i in itr1
        @test !MetidaBase.isnanormissing(i)
    end
    eachindex(itr1)
    eltype(itr1)
    keys(itr1)
    @test length(itr1) == 3

    itr2 = MetidaBase.skipnonpositive(v1)
    for i in itr2
        @test MetidaBase.ispositive(i)
    end
    eachindex(itr2)
    eltype(itr2)
    keys(itr2)
    @test length(itr2) == 2

    #Ststutils
    MetidaBase.sdfromcv(0.4) ≈ 0.38525317015992666
    MetidaBase.varfromcv(0.4) ≈ 0.1484200051182734
    MetidaBase.cvfromvar(0.4) ≈ 0.7013021443295824
    MetidaBase.cvfromsd(0.4) ≈ 0.41654636115540644

end


module TestOldTutorialMissings
include("preamble.jl")

argext_step(should_update) =
    (old, (index, value)) ->
        if old === nothing || should_update(old[2], value)
            (index, value)
        else
            old
        end

xf_scanext(should_update) = Scan(argext_step(should_update), nothing)

slow_xf_fullextrema =
    opcompose(Zip(Count(), NotA(Missing)), Zip(xf_scanext(>), xf_scanext(<)))

slow_xf_argextrema = opcompose(slow_xf_fullextrema, Map() do ((argmin, min), (argmax, max))
    (argmin, argmax)
end)

@testset "slow compilation" begin
    @test foldl(right, slow_xf_fullextrema, [1.0, 3.0, -1.0, missing, 2.0]) ===
        ((3, -1.0), (2, 3.0))

    @test foldl(right, slow_xf_argextrema, [1.0, 3.0, -1.0, missing, 2.0]) ===
        (3, 2)

    # Excluding `missing` from the input vector seems to slow down
    # compilation a lot.
    slow_test("slow compilation test with xf_fullextrema", 20) do
        @test foldl(right, slow_xf_fullextrema, [1.0, 3.0, -1.0, 2.0]) ==
            ((3, -1.0), (2, 3.0))
    end
    slow_test("slow compilation test with xf_argextrema", 20) do
        @test foldl(right, slow_xf_argextrema, [1.0, 3.0, -1.0, 2.0]) ==
            (3, 2)
    end
end

end  # module


module HDParameterGeneratorIO

using NetCDF: NcFile,NcVar,NC_NETCDF4

import HDParameterGenerator: load_input_data,write_hdpara_file
import HDParameterGenerator: Grid,UnstructuredGrid,LatLonGrid

function get_ncells(file_name::AbstractString)
  river_directions::Array{Float64,1} = NetCDF.ncread(file_name,"FDIR")
  return size(river_directions,1)
end

function load_field(file_handle::NcFile,grid::Grid,variable_name::AbstractString,
                    field_type::DataType)
  variable::NcVar = file_handle[variable_name]
  values::Array{field_type,get_number_of_dimensions(grid)} = NetCDF.readvar(variable)
  if isa(grid,LatLonGrid)
    values = permutedims(values, [2,1])
  end
  return values
end

function prep_dims(grid::LatLonGrid)
  lat = NcDim("Lat",grid.nlat)
  lon = NcDim("Lon",grid.nlon)
  dims::Array{NcDim} = NcDim[ lon; lat ]
  return dims
end

function prep_dims(grid::UnstructuredGrid)
  ncells = NcDim("ncells",grid.ncells)
  dims::Array{NcDim} = NcDim[ ncells ]
  return dims
end

function prep_field(dims::Array{NcDim},variable_name::AbstractString,
                    field::Field,fields_to_write::Vector{Pair})
  local variable::NcVar
  if isa(field,LatLonField)
    variable = NcVar(variable_name,dims)
    push!(fields_to_write,Pair(variable,permutedims(get_data(field), [2,1])))
  elseif isa(field,UnstructuredField)
    variable = NcVar(variable_name,dims,atts=Dict("grid_type"=>"unstructured",
                                                  "coordinates"=>"clat clon"))
    push!(fields_to_write,Pair(variable,get_data(field)))
  end
end

function write_fields(grid::Grid,fields_to_write::Vector{Pair},filepath::AbstractString,
                      dims::Array{NcDim})
  variables::Array{NcVar} = NcVar[ pair.first for pair in fields_to_write ]
  if isa(grid,UnstructuredGrid)
    prepare_icon_file(grid,filepath,dims,variables)
  else
    NetCDF.create(filepath,variables,mode=NC_NETCDF4)
  end
  for (variable,data) in fields_to_write
    NetCDF.putvar(variable,data)
  end
end

function prepare_icon_file(grid::UnstructuredGrid,filepath::AbstractString,
                           dims::Array{NcDim},variables::Array{NcVar})
  vertices = NcDim("vertices",3)
  extended_dims::Array{NcDim} = vcat(NcDim[ vertices ], dims)
  clat = NcVar("clat",dims,atts=Dict("long_name"=>"center_latitude",
                                     "units"=>"radians",
                                     "bounds"=>"clat_bnds",
                                     "standard_name"=>"latitude"))
  clon = NcVar("clon",dims,atts=Dict("long_name"=>"center_longitude",
                                     "units"=>"radians",
                                     "bounds"=>"clon_bnds",
                                     "standard_name"=>"longitude"))
  clat_bounds = NcVar("clat_bnds",extended_dims)
  clon_bounds = NcVar("clon_bnds",extended_dims)
  extended_variables::Array{NcVar} = vcat(variables,NcVar[ clat, clon, clat_bounds, clon_bounds])
  NetCDF.create(filepath,extended_variables,mode=NC_NETCDF4)
  NetCDF.putvar(clat,grid.clat)
  NetCDF.putvar(clon,grid.clon)
  NetCDF.putvar(clat_bounds,grid.clat_bounds)
  NetCDF.putvar(clon_bounds,grid.clon_bounds)
end

function write_field(grid::Grid,variable_name::AbstractString,
                     field::Field,filepath::AbstractString)
  dims::Array{NcDim} = prep_dims(grid)
  local variable::NcVar
  if isa(grid,UnstructuredGrid)
    variable = NcVar(variable_name,dims,atts=Dict("grid_type"=>"unstructured",
                                                  "coordinates"=>"clat clon"))
    variables::Array{NcVar} = NcVar[ variable ]
    prepare_icon_file(grid,filepath,dims,variables)
  else
    variable = NcVar(variable_name,dims)
    NetCDF.create(filepath,variable,mode=NC_NETCDF4)
  end
  if isa(grid,LatLonGrid)
    NetCDF.putvar(variable,permutedims(get_data(field), [2,1]))
  else
    NetCDF.putvar(variable,get_data(field))
  end
end

function load_icosahedral_grid(grid_filepath)
  println("Loading: " * grid_filepath)
  file_handle::NcFile = NetCDF.open(grid_filepath)
  ncells::Int64 = file_handle.dim["cell"]
  clat_var::NcVar = file_handle["clat"]
  clat::Array{Float64,1} = readvar(clat_var)
  clon_var::NcVar = file_handle["clon"]
  clon::Array{Float64,1} = readvar(clon_var)
  clat_bounds_var::NcVar = file_handle["clat_bounds"]
  clat_bounds::Array{Float64,2} = readvar(clat_bounds_var)
  clon_bounds_var::NcVar = file_handle["clon_bounds"]
  clon_bounds::Array{Float64,2} = readvar(clon_bounds_var)
  return UnstructuredGrid(ncells,clat,clon,clat_bounds,clon_bounds)
end

function determine_latlon_grid_information(file_handle)
    nlat::Int64 = file_handle.dim["lat"]
    nlon::Int64 = file_handle.dim["lon"]
    lats_var::NcVar = file_handle["lat"]
    lats::Array{Float64,1} = readvar(lats_var)
    lons_var::NcVar = file_handle["lon"]
    lons::Array{Float64,1} = readvar(lons_var)
    return LatLonGrid(nlat,nlon,lats,lons)
end

function load_input_data(input_filepaths::Dict)
  local grid::Grid
  println("Loading: " * input_filepaths["river_directions_filepath"])
  river_directions_file_handle::NcFile = NetCDF.open(input_filepaths["river_directions_filepath"])
  if haskey(input_filepaths,"grid_filepath")
    grid = load_icosahedral_grid(input_filepaths["grid_filepath"])
    river_directions::Array{Int64,ndims} = load_field(river_directions_file_handle,grid,
                                                      input_filepaths["river_directions_fieldname"],
                                                      Int64)
  else
    grid = determine_latlon_grid_information(river_directions_file_handle)
    river_directions::Array{Int64,ndims} =
      round.(Int64,load_field(river_directions_file_handle,grid,
                              input_filepaths["river_directions_fieldname"],Float64)
  end
  ndims::Int64 = size(grid.grid_dimensions)

  println("Loading: " * input_filepaths["orography_variance_filepath"])
  orography_variance_file_handle::NcFile = NetCDF.open(input_filepaths["orography_variance_filepath"]
  orography_variance::Array{Float64,ndims} = load_field(orography_variance_file_handle,grid,
                          input_filepaths["orography_variance__fieldname",Float64)
  println("Loading: " * input_filepaths["innerslope_filepath"])
  innerslope_file_handle::NcFile = NetCDF.open(input_filepaths["innerslope_filepath"]
  innerslope::Array{Float64,ndims} = load_field(innerslope_file_handle,grid,
             input_filepaths["innerslope_fieldname",Float64)
  println("Loading: " * input_filepaths["orography_filepath"])
  orography_file_handle::NcFile = NetCDF.open(input_filepaths["orography_filepath"]
  orography::Array{Float64,ndims} = load_field(orography_file_handle,grid,
                                               input_filepaths["orography_fieldname",
                                               Float64)
  println("Loading: " * input_filepaths["glacier_mask_filepath"])
  glacier_mask_file_handle::NcFile = NetCDF.open(input_filepaths["glacier_mask_filepath"]
  glacier_mask::Array{Bool,ndims} = iszero.(load_field(glacier_mask_file_handle,grid,
                                               input_filepaths["glacier_mask_fieldname",
                                               Int64))
  println("Loading: " * input_filepaths["landsea_mask_filepath"])
  landsea_mask_file_handle::NcFile = NetCDF.open(input_filepaths["landsea_mask_filepath"]
  landsea_mask::Array{Bool,ndims) = iszero.(load_field(landsea_mask_file_handle,grid,
                                            input_filepaths["landsea_mask_fieldname",Float64))
  println("Loading: " * input_filepaths["cell_areas_filepath"])
  cell_areas_file_handle::NcFile = NetCDF.open(input_filepaths["cell_areas_filepath"]
  cell_areas::Array{Float64,ndims} = load_field(cell_areas_file_handle,grid,
                                                input_filepaths["cell_areas_fieldname",
                                                Float64))
  input_data = InputData(landsea_mask,glacier_mask,orography,
                         innerslope,orography_variance,
                         river_directions,cell_areas,grid)
  return input_data::InputData,grid::Grid
end

function write_hdpara_file(output_hdpara_filepath::AbstractString,
                           input_data::InputData,
                           number_of_riverflow_reservoirs{Float64},
                           riverflow_retention_coefficients{Float64},
                           number_of_overlandflow_reservoirs{Float64},
                           overlandflow_retention_coefficients{Float64},
                           number_of_baseflow_reservoirs{Float64},
                           baseflow_retention_coefficients{Float64},
                           grid::Grid)
  dims::Array{NcDim} = prep_dims(grid)
  fields_to_write::Vector{Pair} = Pair[]
  prep_field(dims,"ARF_N",
             number_of_riverflow_reservoirs,fields_to_write)
  prep_field(dims,"ARF_K",
             riverflow_retention_coefficients,fields_to_write)
  prep_field(dims,"ALF_N",
             number_of_overlandflow_reservoirs,fields_to_write)
  prep_field(dims,"ALF_K",
             overlandflow_retention_coefficients,fields_to_write)
  #prep_field(dims,"AGF_N",
  #           number_of_baseflow_reservoirs,fields_to_write)
  prep_field(dims,"AGF_K",
             baseflow_retention_coefficients,fields_to_write)
  prep_field(dims,"Area",
             input_data.cell_areas,fields_to_write)
  if isa(grid,UnstructuredGrid)
    prep_field(dims,"FDIR",
               input_data.grid_specific_input_data.next_cell_index,fields_to_write)
  else
    prep_field(dims,"FDIR",
               input_data.grid_specific_input_data.river_directions,fields_to_write)
  end
  prep_field(dims,"FLAG",
             input_data.landsea_mask,fields_to_write)
  write_fields(grid,fields_to_write,output_hdpara_filepath,dims)
end

end


module LDA

using Vega

include("kernel.jl")
include("lda.jl")
include("visualization.jl")

export lda, process, project
export linear, polynomial, laplacian, rbf
export print_2Ddecision

end # module


## header to provide surface and context
using Cairo
c = CairoRGBSurface(256,256);
cr = CairoContext(c);

save(cr);
set_source_rgb(cr,0.8,0.8,0.8);    # light gray
rectangle(cr,0.0,0.0,256.0,256.0); # background
fill(cr);
restore(cr);

save(cr);
## original example, following here

move_to(cr, 50.0, 75.0);
line_to(cr, 200.0, 75.0);

move_to(cr, 50.0, 125.0);
line_to(cr, 200.0, 125.0);

move_to(cr, 50.0, 175.0);
line_to(cr, 200.0, 175.0);

set_line_width(cr, 30.0);
set_line_cap(cr, Cairo.CAIRO_LINE_CAP_ROUND);
stroke(cr);

## mark picture with current date
restore(cr);
move_to(cr,0.0,12.0);
set_source_rgb(cr, 0,0,0);
show_text(cr,Libc.strftime(time()));
write_to_png(c,"sample_multi_segment_caps.png");


# ============================================================
# Structs of Cell Interface
# compatible with control volume structs
# ============================================================

# ------------------------------------------------------------
# General
# ------------------------------------------------------------

"""
$(TYPEDEF)

Cell interface with no distribution function

# Fields

$(FIELDS)
"""
mutable struct Interface{T,ND} <: AbstractInterface
    fw::T
end

function Interface(fw)
    n = size(fw, 1) - 2
    return Interface{typeof(fw),n}(fw)
end

"""
$(TYPEDEF)

Cell interface with no distribution function

# Fields

$(FIELDS)
"""
struct Interface1F{T1,T2,ND} <: AbstractInterface
    fw::T1
    ff::T2
end

function Interface1F(fw, ff)
    n = size(fw, 1) - 2
    return Interface1F{typeof(fw),typeof(ff),n}(fw, ff)
end

"""
$(TYPEDEF)

Cell interface with no distribution function

# Fields

$(FIELDS)
"""
struct Interface2F{T1,T2,ND} <: AbstractInterface
    fw::T1
    fh::T2
    fb::T2
end

function Interface2F(fw, fh, fb)
    n = size(fw, 1) - 2
    return Interface2F{typeof(fw),typeof(fh),n}(fw, fh, fb)
end

struct Interface3F{T1,T2,ND} <: AbstractInterface end

struct Interface4F{T1,T2,ND} <: AbstractInterface end

"""
$(SIGNATURES)

Construct cell interface
"""
function Interface(W, ND::Integer)
    fw = zero(W)

    return Interface{typeof(fw),ND}(fw)
end

"""
$(SIGNATURES)
"""
function Interface(W, F, ND::Integer)
    fw = zero(W)
    ff = zero(F)

    return Interface1F{typeof(fw),typeof(ff),ND}(fw, ff)
end

"""
$(SIGNATURES)
"""
function Interface(W, H, B, ND::Integer)
    fw = deepcopy(W)
    fh = zero(H)
    fb = zero(B)

    return Interface2F{typeof(fw),typeof(fh),ND}(fw, fh, fb)
end

# ------------------------------------------------------------
# The dimension-dependent structures are in archive mode only
# ------------------------------------------------------------
# 1D
# ------------------------------------------------------------

"""
    mutable struct Interface1D{A<:Union{Real,AA}} <: AbstractInterface1D
        fw::A
    end

1D cell interface with no distribution function
Note that deepcopy constructor is needed to work with StructArrays

"""
mutable struct Interface1D{A<:Union{Real,AA}} <: AbstractInterface1D
    fw::A
end

function Base.show(io::IO, ctr::Interface1D{A}) where {A}
    print(io, "Interface1D{$A}\n", "conservative fluxes: $(ctr.fw)\n")
end

"""
    mutable struct Interface1D1F{A,B} <: AbstractInterface1D
        fw::A
        ff::B
    end

1D cell interface with 1 distribution function
Note that deepcopy constructor is needed to work with StructArrays

"""
mutable struct Interface1D1F{A,B<:AA} <: AbstractInterface1D
    fw::A
    ff::B
end

function Base.show(io::IO, ctr::Interface1D1F{A,B}) where {A,B}
    print(
        io,
        "Interface1D1F{$A,$B}\n",
        "conservative fluxes: $(ctr.fw)\n",
        "pdf fluxes: ff\n",
    )
end

"""
    struct Interface1D2F{A,B} <: AbstractInterface1D
        fw::A
        fh::B
        fb::B

        function Interface1D2F(w::AA, f::AA)
            fw = zero(w)
            fh = zero(f)
            fb = zero(f)

            new{typeof(fw),typeof(fh)}(fw, fh, fb)
        end
    end

1D cell interface with 2 distribution functions

"""
struct Interface1D2F{A,B<:AA} <: AbstractInterface1D
    fw::A
    fh::B
    fb::B
end

function Interface1D2F(w, f::AA)
    fw = zero(w)
    fh = zero(f)
    fb = zero(f)

    return Interface1D2F(fw, fh, fb)
end

function Base.show(io::IO, ctr::Interface1D2F{A,B}) where {A,B}
    print(
        io,
        "Interface1D2F{$A,$B}\n",
        "conservative fluxes: $(ctr.fw)\n",
        "pdf fluxes: fh, fb\n",
    )
end

"""
    struct Interface1D3F{A,B,C} <: AbstractInterface1D
        fw::A
        fh0::B
        fh1::B
        fh2::B
        femL::C
        femR::C
    end

1D cell interface with 3 distribution functions

"""
struct Interface1D3F{A,B,C} <: AbstractInterface1D
    fw::A
    fh0::B
    fh1::B
    fh2::B
    femL::C
    femR::C
end

# deterministic
function Interface1D3F(w::AA, f::AA, E::AA{<:Real,1})
    fw = zeros(eltype(w), axes(w))
    fh0 = zeros(eltype(f), axes(f))
    fh1 = zeros(eltype(f), axes(f))
    fh2 = zeros(eltype(f), axes(f))
    femL = zeros(eltype(E), 8)
    femR = zeros(eltype(E), 8)

    return Interface1D3F(fw, fh0, fh1, fh2, femL, femR)
end

# stochastic
function Interface1D3F(w::AA, f::AA, E::AA{<:Real,2})
    fw = zeros(eltype(w), axes(w))
    fh0 = zeros(eltype(f), axes(f))
    fh1 = zeros(eltype(f), axes(f))
    fh2 = zeros(eltype(f), axes(f))
    femL = zeros(eltype(E), 8, axes(E, 2))
    femR = zeros(eltype(E), 8, axes(E, 2))

    return Interface1D3F(fw, fh0, fh1, fh2, femL, femR)
end

# Rykov
function Interface1D3F(w::AA, f::AA)
    fw = zero(w)
    fh0 = zero(f)
    fh1 = zero(f)
    fh2 = zero(f)
    femL = nothing
    femR = nothing

    return Interface1D3F(fw, fh0, fh1, fh2, femL, femR)
end

function Base.show(io::IO, ctr::Interface1D3F{A,B,C}) where {A,B,C}
    print(
        io,
        "Interface1D3F{$A,$B,$C}\n",
        "conservative fluxes: $(ctr.fw)\n",
        "pdf fluxes: fh0, fh1, fh2\n",
        "electromagnetic fluxes: femL, femR\n",
    )
end

"""
    struct Interface1D4F{A,B,C} <: AbstractInterface1D
        fw::A
        fh0::B
        fh1::B
        fh2::B
        fh3::B
        femL::C
        femR::C
    end

1D cell interface with 4 distribution functions

"""
struct Interface1D4F{A,B,C} <: AbstractInterface1D
    fw::A
    fh0::B
    fh1::B
    fh2::B
    fh3::B
    femL::C
    femR::C
end

# deterministic
function Interface1D4F(w::AA, f::AA, E::AA{<:Real,1})
    fw = zeros(eltype(w), axes(w))
    fh0 = zeros(eltype(f), axes(f))
    fh1 = zeros(eltype(f), axes(f))
    fh2 = zeros(eltype(f), axes(f))
    fh3 = zeros(eltype(f), axes(f))
    femL = zeros(eltype(E), 8)
    femR = zeros(eltype(E), 8)

    return Interface1D4F(fw, fh0, fh1, fh2, fh3, femL, femR)
end

# stochastic
function Interface1D4F(w::AA, f::AA, E::AA{<:Real,2})
    fw = zeros(eltype(w), axes(w))
    fh0 = zeros(eltype(f), axes(f))
    fh1 = zeros(eltype(f), axes(f))
    fh2 = zeros(eltype(f), axes(f))
    fh3 = zeros(eltype(f), axes(f))
    femL = zeros(eltype(E), 8, axes(E, 2))
    femR = zeros(eltype(E), 8, axes(E, 2))

    return Interface1D4F(fw, fh0, fh1, fh2, fh3, femL, femR)
end

function Base.show(io::IO, ctr::Interface1D4F{A,B,C}) where {A,B,C}
    print(
        io,
        "Interface1D4F{$A,$B,$C}\n",
        "conservative fluxes: $(ctr.fw)\n",
        "pdf fluxes: fh0, fh1, fh2, fh3\n",
        "electromagnetic fluxes: femL, femR\n",
    )
end

# ------------------------------------------------------------
# 2D
# ------------------------------------------------------------

"""
    mutable struct Interface2D{A,B,C} <: AbstractInterface2D
        len::A
        n::B
        fw::C

        function Interface2D(L::Real, C::Real, S::Real, w::AA)
            len = L
            n = [C, S]

            fw = zero(w)

            new{typeof(len),typeof(n),typeof(fw)}(len, n, fw)
        end
    end

2D cell interface with no distribution function

"""
mutable struct Interface2D{A,B,C} <: AbstractInterface2D
    len::A
    n::B
    fw::C
end

function Interface2D(L::Real, C::Real, S::Real, w::AA)
    len = L
    n = [C, S]

    fw = zero(w)

    return Interface2D(len, n, fw)
end

function Base.show(io::IO, ctr::Interface2D{A,B,C}) where {A,B,C}
    print(
        io,
        "Interface2D{$A,$B,$C}\n",
        "length: $(ctr.len)\n",
        "normal vector: ($(ctr.n[1]),$(ctr.n[2]))\n",
        "conservative fluxes: $(ctr.fw)\n",
    )
end

"""
    mutable struct Interface2D1F{A,B,C,D} <: AbstractInterface2D
        len::A
        n::B
        fw::C
        ff::D

        function Interface2D1F(L::Real, C::Real, S::Real, w::AA, f::AA)
            len = L
            n = [C, S]

            fw = zero(w)
            ff = zero(f)

            new{typeof(len),typeof(n),typeof(fw),typeof(ff)}(len, n, fw, ff)
        end
    end

2D cell interface with 1 distribution function

"""
mutable struct Interface2D1F{A,B,C,D} <: AbstractInterface2D
    len::A
    n::B
    fw::C
    ff::D
end

function Interface2D1F(L::Real, C::Real, S::Real, w, f::AA)
    len = L
    n = [C, S]

    fw = zero(w)
    ff = zero(f)

    return Interface2D1F(len, n, fw, ff)
end

function Base.show(io::IO, ctr::Interface2D1F{A,B,C,D}) where {A,B,C,D}
    print(
        io,
        "Interface2D1F{$A,$B,$C}\n",
        "length: $(ctr.len)\n",
        "normal vector: ($(ctr.n[1]),$(ctr.n[2]))\n",
        "conservative fluxes: $(ctr.fw)\n",
        "pdf fluxes: ff",
    )
end

"""
    struct Interface2D2F{A,B,C,D} <: AbstractInterface2D
        len::A
        n::B
        fw::C
        fh::D
        fb::D

        function Interface2D2F(L::Real, C::Real, S::Real, w::AA, f::AA)
            len = L
            n = [C, S]

            fw = zeros(eltype(w), axes(w))
            fh = zeros(eltype(f), axes(f))
            fb = zeros(eltype(f), axes(f))

            new{typeof(len),typeof(n),typeof(fw),typeof(fh)}(len, n, fw, fh, fb)
        end
    end

2D cell interface with 2 distribution functions

"""
struct Interface2D2F{A,B<:AV,C,D} <: AbstractInterface2D
    len::A
    n::B
    fw::C
    fh::D
    fb::D
end

function Interface2D2F(L::Real, C::Real, S::Real, w::AA, f::AA)
    len = L
    n = @SArray [C, S]

    fw = zero(w)
    fh = zero(f)
    fb = zero(f)

    return Interface2D2F(len, n, fw, fh, fb)
end

function Base.show(io::IO, ctr::Interface2D2F{A,B,C,D}) where {A,B,C,D}
    print(
        io,
        "Interface2D2F{$A,$B,$C}\n",
        "length: $(ctr.len)\n",
        "normal vector: ($(ctr.n[1]),$(ctr.n[2]))\n",
        "conservative fluxes: $(ctr.fw)\n",
        "pdf fluxes: fh, fb",
    )
end

"""
    struct Interface2D3F{A,B,C,D,E} <: AbstractInterface2D
        len::A
        n::B
        fw::C
        fh0::D
        fh1::D
        fh2::D
        femL::E
        femR::E
        femLU::E
        femLD::E
        femRU::E
        femRD::E
    end

2D cell interface with 3 distribution functions

"""
struct Interface2D3F{A,B,C,D,E} <: AbstractInterface2D
    len::A
    n::B
    fw::C
    fh0::D
    fh1::D
    fh2::D
    femL::E
    femR::E
    femLU::E
    femLD::E
    femRU::E
    femRD::E
end

function Interface2D3F(L::Real, C::Real, S::Real, w::AA, f::AA, E::AA{<:Real,1})
    len = L
    n = @SArray [C, S]

    fw = zero(w)
    fh0 = zero(f)
    fh1 = zero(f)
    fh2 = zero(f)

    femL = zeros(eltype(E), 8)
    femR = zeros(eltype(E), 8)
    femLU = zeros(eltype(E), 8)
    femLD = zeros(eltype(E), 8)
    femRU = zeros(eltype(E), 8)
    femRD = zeros(eltype(E), 8)

    return Interface2D3F(len, n, fw, fh0, fh1, fh2, femL, femR, femLU, femLD, femRU, femRD)
end

function Interface2D3F(L::Real, C::Real, S::Real, w::AA, f::AA, E::AA{<:Real,2})
    len = L
    n = [C, S]

    fw = zero(w)
    fh0 = zero(f)
    fh1 = zero(f)
    fh2 = zero(f)

    femL = zeros(eltype(E), 8, axes(E, 2))
    femR = zeros(eltype(E), 8, axes(E, 2))
    femLU = zeros(eltype(E), 8, axes(E, 2))
    femLD = zeros(eltype(E), 8, axes(E, 2))
    femRU = zeros(eltype(E), 8, axes(E, 2))
    femRD = zeros(eltype(E), 8, axes(E, 2))

    return Interface2D3F(len, n, fw, fh0, fh1, fh2, femL, femR, femLU, femLD, femRU, femRD)
end

# Rykov
function Interface2D3F(L::Real, C::Real, S::Real, w::AA, f::AA)
    len = L
    n = [C, S]

    fw = zero(w)
    fh0 = zero(f)
    fh1 = zero(f)
    fh2 = zero(f)

    femL = nothing
    femR = nothing
    femLU = nothing
    femLD = nothing
    femRU = nothing
    femRD = nothing

    return Interface2D3F(len, n, fw, fh0, fh1, fh2, femL, femR, femLU, femLD, femRU, femRD)
end

function Base.show(io::IO, ctr::Interface2D3F{A,B,C,D,E}) where {A,B,C,D,E}
    print(
        io,
        "Interface2D3F{$A,$B,$C}\n",
        "length: $(ctr.len)\n",
        "normal vector: ($(ctr.n[1]),$(ctr.n[2]))\n",
        "conservative fluxes: $(ctr.fw)\n",
        "pdf fluxes: fh0, fh1, fh2\n",
        "electromagnetic fluxes: femL, femR, femLU, femLD, femRU, femRD\n",
    )
end


MakieLayout.@Layoutable LTeX

function MakieLayout.default_attributes(::Type{LTeX}, scene)
    Attributes(
        tex = raw"\int \mathbf E \cdot d\mathbf a = \frac{Q_{encl}}{4\pi\epsilon_0}",
        visible = true,
        padding = (0f0, 0f0, 0f0, 0f0),
        height = Auto(),
        width = Auto(),
        alignmode = Inside(),
        valign = :center,
        halign = :center,
        tellwidth = true,
        tellheight = true,
        dpi = 72.0,
        textsize = 12,
        rotation = 0
    )
end

function MakieLayout.layoutable(::Type{LTeX}, fig_or_scene, tex; kwargs...)
    MakieLayout.layoutable(LTeX, fig_or_scene; tex = tex, kwargs...)
end

function MakieLayout.layoutable(::Type{LTeX}, fig_or_scene; bbox = nothing, kwargs...)
    # this is more or less copied from MakieLayout.Label
    topscene = MakieLayout.get_topscene(fig_or_scene)
    default_attrs = MakieLayout.default_attributes(LTeX, topscene).attributes
    theme_attrs = MakieLayout.subtheme(topscene, :LTeX)
    attrs = MakieLayout.merge!(MakieLayout.merge!(Attributes(kwargs), theme_attrs), Attributes(default_attrs))

    # @extract attrs (tex, textsize, font, color, visible, halign, valign,
    #     rotation, padding)
    @extract attrs (tex, dpi, textsize, visible, padding, halign, valign, rotation)

    layoutobservables = LayoutObservables{LTeX}(
        attrs.width, attrs.height, attrs.tellwidth, attrs.tellheight,
        halign, valign, attrs.alignmode; suggestedbbox = bbox
    )

    # This is Point3f0 in Label
    textpos = Node(Point3f0(0))

    cached_tex = @lift CachedTeX($tex, $dpi)

    # this is just a hack until boundingboxes in abstractplotting are perfect
    alignnode = lift(halign, rotation) do h, rot
        # left align the text if it's not rotated and left aligned
        if rot == 0 && (h == :left || h == 0.0)
            (:left, :center)
        else
            (:center, :center)
        end
    end

    t = teximg!(
        topscene, cached_tex; position = textpos, visible = visible, raw = true,
        textsize = textsize, dpi = dpi, align = alignnode, rotation = rotation
    )

    textbb = Ref(BBox(0, 1, 0, 1))

    # Label
    onany(cached_tex, textsize, rotation, padding) do _, textsize, rotation, padding
        # rotation is applied via a model matrix which isn't used in the bbox calculation
        # so we need to deal with it here
        bb = boundingbox(t)
        R = Makie.Mat3f0(
             cos(rotation), sin(rotation), 0,
            -sin(rotation), cos(rotation), 0,
            0, 0, 1
        )
        points = map(p -> R * p, unique(Makie.coordinates(bb)))
        new_bb = Makie.xyz_boundingbox(identity, points)
        textbb[] = FRect2D(new_bb)
        # textbb[] = FRect2D(boundingbox(t))
        autowidth  = Makie.width(textbb[]) + padding[1] + padding[2]
        autoheight = Makie.height(textbb[]) + padding[3] + padding[4]
        layoutobservables.autosize[] = (autowidth, autoheight)
    end

    onany(layoutobservables.computedbbox, padding) do bbox, padding
        tw = Makie.width(textbb[])
        th = Makie.height(textbb[])

        box = bbox.origin[1]
        boy = bbox.origin[2]

        # this is also part of the hack to improve left alignment until
        # boundingboxes are perfect
        tx = if rotation[] == 0 && (halign[] == :left || halign[] == 0.0)
            box + padding[1]
        else
            box + padding[1] + 0.5 * tw
        end
        ty = boy + padding[3] + 0.5 * th

        textpos[] = Point3f0(tx, ty, 0)
    end

    # trigger first update, otherwise bounds are wrong somehow
    padding[] = padding[]
    # trigger bbox
    layoutobservables.suggestedbbox[] = layoutobservables.suggestedbbox[]

    lt = LTeX(fig_or_scene, layoutobservables, attrs, Dict(:tex => t))

    lt
end


# # Feature Extraction

#md # [![](https://img.shields.io/badge/show-nbviewer-579ACA.svg)](@__NBVIEWER_ROOT_URL__/notebooks/feature_extraction.ipynb)

# In this example we demonstrate how to extract feature from trajectories using AutomotiveSimulator

# **Load a Dataset**
# First let's create a synthetic dataset.

using AutomotiveSimulator
using AutomotiveVisualization
AutomotiveVisualization.colortheme["background"] = colorant"white"; # hide
using Random

roadway = roadway = gen_straight_roadway(3, 1000.0)
veh_state = VehicleState(Frenet(roadway[LaneTag(1,2)], 0.0), roadway, 10.)
veh1 = Entity(veh_state, VehicleDef(), "bob")
veh_state = VehicleState(Frenet(roadway[LaneTag(1,2)], 20.0), roadway, 2.)
veh2 = Entity(veh_state, VehicleDef(), "alice")

dt = 0.5
n_steps = 10
models = Dict{String, DriverModel}()
models["bob"] = Tim2DDriver(mlane=MOBIL())
set_desired_speed!(models["bob"], 10.0)
models["alice"] = Tim2DDriver(mlane=MOBIL())
set_desired_speed!(models["alice"], 2.0)

scene = Scene([veh1, veh2])
scenes = simulate(scene, roadway, models, n_steps, dt)
camera = SceneFollowCamera()
update_camera!(camera, scene)
snapshot = render([roadway, scene], camera=camera)
#md write("feature_initial.svg", snapshot) # hide
#md # ![initial state of feature extraction scenario](feature_initial.svg)

#md nothing # hide

# One can also load the data from the Stadium tutorial

#md load data from stadium tutorial
#md scenes = open("2Dstadium_listrec.txt", "r") do io
#md     read(io, Vector{EntityScene{VehicleState, VehicleDef, String}})
#md end

# **Extract features from a recorded trajectory**
# Recorded trajectories are expected to be vectors of `Scene`s where each element correspond to one time step.
# To extract features, one can use the `extract_features` function which takes as input
# a list of feature we want to extract and the list of vehicle ids for which we want those features.
# For this example, let's first query two features, the longitudinal and lateral position of Bob, and whether or not Bob is colliding:

dfs = extract_features((posfs, posft, iscolliding), roadway, scenes, ["bob"])
dfs["bob"]

# To query features for all traffic participants we can just add their ID to the list:

dfs = extract_features((posfs, posft, iscolliding), roadway, scenes, ["bob", "alice"])
dfs["alice"]

# The output is a dictionary mapping ID to dataframes. To learn more about DataFrames visit [DataFrames.jl](https://github.com/JuliaData/DataFrames.jl).

# For the list of all possible features available see the documentation.
# Features are generally just function. AutomotiveSimulator provides some convenience
# to automatically generate feature function like `distance_to_$x`
# The `distance_to` function takes as input a vehicle ID and returns a function to extract
# the distance between the queried vehicle and the vehicle given to `distance_to`

distance_to("alice")

# we can use this newly generated funciton in the feature extraction pipeline

dfs = extract_features((distance_to_alice, posfs), roadway, scenes, ["bob"])
dfs["bob"].distance_to_alice[1] # distance between Bob and Alice in the first scene.


VERSION < v"0.7.0-beta2.199" && __precompile__()

module NLopt

export Opt, NLOPT_VERSION, algorithm, algorithm_name, ForcedStop,
       lower_bounds!, lower_bounds, upper_bounds!, upper_bounds, stopval!, stopval, ftol_rel!, ftol_rel, ftol_abs!, ftol_abs, xtol_rel!, xtol_rel, xtol_abs!, xtol_abs, maxeval!, maxeval, maxtime!, maxtime, force_stop!, force_stop, force_stop!, population!, population, vector_storage!, vector_storage, initial_step!, initial_step, default_initial_step!, local_optimizer!,
       min_objective!, max_objective!, equality_constraint!, inequality_constraint!, remove_constraints!,
       optimize!, optimize, Algorithm, Result

import MathProgBase.SolverInterface
import MathProgBase.SolverInterface.optimize!

import Libdl
const depsjl_path = joinpath(dirname(@__FILE__), "..", "deps", "deps.jl")
if !isfile(depsjl_path)
    error("NLopt not installed properly; run Pkg.build(\"NLopt\"), restart Julia, and try again.")
end
include(depsjl_path)

function __init__()
    check_deps()
    v = version()
    v >= v"2.5" || error("NLopt $v < 2.5 is too old")
end

############################################################################
# Mirrors of NLopt's C enum constants:

@enum Algorithm::Cint begin
    GN_DIRECT=0
    GN_DIRECT_L=1
    GN_DIRECT_L_RAND=2
    GN_DIRECT_NOSCAL=3
    GN_DIRECT_L_NOSCAL=4
    GN_DIRECT_L_RAND_NOSCAL=5
    GN_ORIG_DIRECT=6
    GN_ORIG_DIRECT_L=7
    GD_STOGO=8
    GD_STOGO_RAND=9
    LD_LBFGS_NOCEDAL=10
    LD_LBFGS=11
    LN_PRAXIS=12
    LD_VAR1=13
    LD_VAR2=14
    LD_TNEWTON=15
    LD_TNEWTON_RESTART=16
    LD_TNEWTON_PRECOND=17
    LD_TNEWTON_PRECOND_RESTART=18
    GN_CRS2_LM=19
    GN_MLSL=20
    GD_MLSL=21
    GN_MLSL_LDS=22
    GD_MLSL_LDS=23
    LD_MMA=24
    LN_COBYLA=25
    LN_NEWUOA=26
    LN_NEWUOA_BOUND=27
    LN_NELDERMEAD=28
    LN_SBPLX=29
    LN_AUGLAG=30
    LD_AUGLAG=31
    LN_AUGLAG_EQ=32
    LD_AUGLAG_EQ=33
    LN_BOBYQA=34
    GN_ISRES=35
    AUGLAG=36
    AUGLAG_EQ=37
    G_MLSL=38
    G_MLSL_LDS=39
    LD_SLSQP=40
    LD_CCSAQ=41
    GN_ESCH=42
    GN_AGS=43
end

const sym2alg = Dict(Symbol(i)=>i for i in instances(Algorithm))

# enum nlopt_result
@enum Result::Cint begin
    FORCED_STOP=-5
    ROUNDOFF_LIMITED=-4
    OUT_OF_MEMORY=-3
    INVALID_ARGS=-2
    FAILURE=-1
    SUCCESS=1
    STOPVAL_REACHED=2
    FTOL_REACHED=3
    XTOL_REACHED=4
    MAXEVAL_REACHED=5
    MAXTIME_REACHED=6
end

# so that result < 0 checks continue to work
Base.isless(x::Integer, r::Result) = isless(x, Cint(r))
Base.isless(r::Result, x::Integer) = isless(Cint(r), x)
# so that == :Foo checks continue to work
Base.:(==)(s::Symbol, r::Result) = s == Symbol(r)
Base.:(==)(r::Result, s::Symbol) = s == r

############################################################################
# wrapper around nlopt_opt type

const _Opt = Ptr{Cvoid} # nlopt_opt

# pass both f and o to the callback so that we can handle exceptions
struct Callback_Data
    f::Function
    o::Any # should be Opt, but see Julia issue #269
end

mutable struct Opt
    opt::_Opt

    # need to store callback data for objective and constraints in
    # Opt so that they aren't garbage-collected.  cb[1] is the objective.
    cb::Vector{Callback_Data}

    function Opt(p::_Opt)
        opt = new(p, Array{Callback_Data}(undef,1))
        finalizer(destroy,opt)
        opt
    end
    function Opt(algorithm::Algorithm, n::Integer)
        n >= 0 || throw(ArgumentError("invalid dimension $n < 0"))
        p = ccall((:nlopt_create,libnlopt), _Opt, (Algorithm, Cuint),
                  algorithm, n)
        if p == C_NULL
            error("Error in nlopt_create")
        end
        Opt(p)
    end
    Opt(alg::Integer, n::Integer) = Opt(Algorithm(alg), n)
    Opt(algorithm::Symbol, n::Integer) = Opt(try sym2alg[algorithm]
                                             catch
                         throw(ArgumentError("unknown algorithm $algorithm"))
                                             end, n)
end

Base.unsafe_convert(::Type{_Opt}, o::Opt) = getfield(o, :opt) # for passing to ccall

destroy(o::Opt) = ccall((:nlopt_destroy,libnlopt), Cvoid, (_Opt,), o)

Base.ndims(o::Opt) = Int(ccall((:nlopt_get_dimension,libnlopt), Cuint, (_Opt,), o))
algorithm(o::Opt) = ccall((:nlopt_get_algorithm,libnlopt), Algorithm, (_Opt,), o)

Base.show(io::IO, o::Opt) = print(io, "Opt($(algorithm(o)), $(ndims(o)))")

############################################################################
# copying is a little tricky because we have to tell NLopt to use
# new Callback_Data.

# callback wrapper for nlopt_munge_data in NLopt 2.4
function munge_callback(p::Ptr{Cvoid}, f_::Ptr{Cvoid})
    f = unsafe_pointer_to_objref(f_)::Function
    f(p)::Ptr{Cvoid}
end

function Base.copy(o::Opt)
    p = ccall((:nlopt_copy,libnlopt), _Opt, (_Opt,), o)
    if p == C_NULL
        error("Error in nlopt_copy")
    end
    n = Opt(p)

    cb = getfield(o.cb)
    n.cb = similar(cb)
    for i = 1:length(cb)
        try
            n.cb[i] = Callback_Data(cb[i].f, n)
        catch e
            # if objective has not been set, cb[1] will throw
            # an UndefRefError, which is okay.
            if i != 1 || !isa(e, UndefRefError)
                rethrow(e) # some not-okay exception
            end
        end
    end

    try
        # n.o, for each callback, stores a pointer to an element of o.cb,
        # and we need to convert this into a pointer to the corresponding
        # element of n.cb.  nlopt_munge_data allows us to call a function
        # to transform each stored pointer in n.o, and we use the cbi
        # dictionary to convert pointers to indices into o.cb, whence
        # we obtain the corresponding element of n.cb.
        cbi = Dict{Ptr{Cvoid},Int}()
        for i in 1:length(cb)
            try
                cbi[pointer_from_objref(cb[i])] = i
            catch
            end
        end
        munge_callback_ptr = @cfunction(munge_callback, Ptr{Cvoid},
                                        (Ptr{Cvoid}, Ptr{Cvoid}))
        ccall((:nlopt_munge_data,libnlopt), Cvoid, (_Opt, Ptr{Cvoid}, Any),
              n, munge_callback_ptr,
              p::Ptr{Cvoid} -> p==C_NULL ? C_NULL :
                              pointer_from_objref(n.cb[cbi[p]]))
    catch e0
        # nlopt_munge_data not available, punt unless there is
        # no callback data
        try
            cb[1]
        catch e
            if length(cb) == 1 && isa(e, UndefRefError)
                return n
            end
        end
        error("copy(o::Opt) not supported for NLopt version < 2.4")
    end

    return n
end

############################################################################
# converting error results into exceptions

struct ForcedStop <: Exception end

# cache current exception for forced stop
nlopt_exception = nothing

function errmsg(o::Opt)
    msg = ccall((:nlopt_get_errmsg,libnlopt), Ptr{UInt8}, (_Opt,), o)
    return msg == C_NULL ? nothing : unsafe_string(msg)
end

function _errmsg(o::Opt)
    s = errmsg(o)
    return s === nothing || isempty(s) ? "" : ": "*s
end

# check result and throw an exception if necessary
function chk(o::Opt, result::Result)
    if result < 0 && result != ROUNDOFF_LIMITED
        if result == INVALID_ARGS
            throw(ArgumentError("invalid NLopt arguments"*_errmsg(o)))
        elseif result == OUT_OF_MEMORY
            throw(OutOfMemoryError())
        elseif result == FORCED_STOP
            global nlopt_exception
            e = nlopt_exception
            if e !== nothing && !isa(e, ForcedStop)
                nlopt_exception = nothing
                rethrow(e)
            end
        else
            error("nlopt failure $result", _errmsg(o))
        end
    end
    return nothing
end

############################################################################
# getting and setting scalar and vector parameters

# make a quoted symbol expression out of the arguments
qsym(args...) = Expr(:quote, Symbol(string(args...)))

# scalar parameters p of type T
macro GETSET(T, p)
    Tg = T == :Cdouble ? :Real : (T == :Cint || T == :Cuint ? :Integer : :Any)
    ps = Symbol(string(p, "!"))
    quote
        $(esc(p))(o::Opt) = ccall(($(qsym("nlopt_get_", p)),libnlopt),
                                  $T, (_Opt,), o)
        $(esc(ps))(o::Opt, val::$Tg) =
          chk(o, ccall(($(qsym("nlopt_set_", p)),libnlopt),
                     Result, (_Opt, $T), o, val))
    end
end

# Vector{Cdouble} parameters p
macro GETSET_VEC(p)
    ps = Symbol(string(p, "!"))
    quote
        function $(esc(p))(o::Opt, v::Vector{Cdouble})
            if length(v) != ndims(o)
                throw(BoundsError())
            end
            chk(o, ccall(($(qsym("nlopt_get_", p)),libnlopt),
                      Result, (_Opt, Ptr{Cdouble}), o, v))
            v
        end
        $(esc(p))(o::Opt) = $(esc(p))(o, Array{Cdouble}(undef, ndims(o)))
        function $(esc(ps))(o::Opt, v::Vector{Cdouble})
            if length(v) != ndims(o)
                throw(BoundsError())
            end
            chk(o, ccall(($(qsym("nlopt_set_", p)),libnlopt),
                      Result, (_Opt, Ptr{Cdouble}), o, v))
        end
        $(esc(ps))(o::Opt, v::AbstractVector{<:Real}) =
          $(esc(ps))(o, Array{Cdouble}(v))
        $(esc(ps))(o::Opt, val::Real) =
          chk(o, ccall(($(qsym("nlopt_set_", p, "1")),libnlopt),
                     Result, (_Opt, Cdouble), o, val))
    end
end

############################################################################
# Optimizer parameters

@GETSET_VEC lower_bounds
@GETSET_VEC upper_bounds
@GETSET Cdouble stopval
@GETSET Cdouble ftol_rel
@GETSET Cdouble ftol_abs
@GETSET Cdouble xtol_rel
@GETSET_VEC xtol_abs
@GETSET Cint maxeval
@GETSET Cdouble maxtime
@GETSET Cint force_stop
@GETSET Cuint population
@GETSET Cuint vector_storage

force_stop!(o::Opt) = force_stop!(o, 1)

local_optimizer!(o::Opt, lo::Opt) =
  chk(o, ccall((:nlopt_set_local_optimizer,libnlopt),
             Result, (_Opt, _Opt), o, lo))

# the initial-stepsize stuff is a bit different than GETSET_VEC,
# since the heuristics depend on the position x.

function default_initial_step!(o::Opt, x::Vector{Cdouble})
    if length(x) != ndims(o)
        throw(BoundsError())
    end
    chk(o, ccall((:nlopt_set_default_initial_step,libnlopt),
               Result, (_Opt, Ptr{Cdouble}), o, x))
end
default_initial_step!(o::Opt, x::AbstractVector{<:Real}) =
  default_initial_step!(o, Array{Cdouble}(x))

function initial_step!(o::Opt, dx::Vector{Cdouble})
    if length(dx) != ndims(o)
        throw(BoundsError())
    end
    chk(o, ccall((:nlopt_set_initial_step,libnlopt),
               Result, (_Opt, Ptr{Cdouble}), o, dx))
end
initial_step!(o::Opt, dx::AbstractVector{<:Real}) =
  initial_step!(o, Array{Cdouble}(dx))
initial_step!(o::Opt, dx::Real) =
  chk(o, ccall((:nlopt_set_initial_step1,libnlopt),
             Result, (_Opt, Cdouble), o, dx))

function initial_step(o::Opt, x::Vector{Cdouble}, dx::Vector{Cdouble})
    if length(x) != ndims(o) || length(dx) != ndims(o)
        throw(BoundsError())
    end
    chk(o, ccall((:nlopt_get_initial_step,libnlopt),
               Result, (_Opt, Ptr{Cdouble}, Ptr{Cdouble}), o, x, dx))
    dx
end
initial_step(o::Opt, x::AbstractVector{<:Real}) =
    initial_step(o, Array{Cdouble}(x),
                 Array{Cdouble}(undef, ndims(o)))

############################################################################

function algorithm_name(a::Algorithm)
    s = ccall((:nlopt_algorithm_name,libnlopt), Ptr{UInt8}, (Algorithm,), a)
    if s == C_NULL
        throw(ArgumentError("invalid algorithm $a"))
    end
    return unsafe_string(s)
end

algorithm_name(a::Integer) = algorithm_name(Algorithm(a))
algorithm_name(a::Symbol) = algorithm_name(try sym2alg[a]
                                           catch
                             throw(ArgumentError("unknown algorithm $a"))
                                           end)
algorithm_name(o::Opt) = algorithm_name(algorithm(o))

function Base.show(io::IO, ::MIME"text/plain", a::Algorithm)
    show(io, a)
    print(io, ": ", algorithm_name(a))
end

numevals(o::Opt) = ccall((:nlopt_get_numevals,libnlopt), Cint, (_Opt,), o)

############################################################################

function version()
    major = Ref{Cint}()
    minor = Ref{Cint}()
    patch = Ref{Cint}()
    ccall((:nlopt_version,libnlopt), Cvoid, (Ref{Cint},Ref{Cint},Ref{Cint}),
          major, minor, patch)
    return VersionNumber(major[], minor[], patch[])
end

const NLOPT_VERSION = version()

############################################################################

srand(seed::Integer) = ccall((:nlopt_srand,libnlopt),
                             Cvoid, (Culong,), seed)
srand_time() = ccall((:nlopt_srand_time,libnlopt), Cvoid, ())

############################################################################
# Objective function:

const empty_grad = Cdouble[] # for passing when grad == C_NULL

function nlopt_callback_wrapper(n::Cuint, x::Ptr{Cdouble},
                                grad::Ptr{Cdouble}, d_::Ptr{Cvoid})
    d = unsafe_pointer_to_objref(d_)::Callback_Data
    try
        res = convert(Cdouble,
                      d.f(unsafe_wrap(Array, x, (convert(Int, n),)),
                          grad == C_NULL ? empty_grad
                          : unsafe_wrap(Array, grad, (convert(Int, n),))))
        return res::Cdouble
    catch e
        global nlopt_exception
        nlopt_exception = e
        force_stop!(d.o::Opt)
        return 0.0 # ignored by nlopt
    end
end

for m in (:min, :max)
    mf = Symbol(string(m,"_objective!"))
    @eval function $mf(o::Opt, f::Function)
        getfield(o, :cb)[1] = Callback_Data(f, o)
        nlopt_callback_wrapper_ptr = @cfunction(nlopt_callback_wrapper,
            Cdouble, (Cuint, Ptr{Cdouble}, Ptr{Cdouble}, Ptr{Cvoid}))
        chk(o, ccall(($(qsym("nlopt_set_", m, "_objective")),libnlopt),
                   Result, (_Opt, Ptr{Cvoid}, Any),
                   o, nlopt_callback_wrapper_ptr,
                   getfield(o, :cb)[1]))
    end
end

############################################################################
# Nonlinear constraints:

for c in (:inequality, :equality)
    cf = Symbol(string(c, "_constraint!"))
    @eval function $cf(o::Opt, f::Function, tol::Real=0.0)
        push!(getfield(o, :cb), Callback_Data(f, o))
        nlopt_callback_wrapper_ptr = @cfunction(nlopt_callback_wrapper,
            Cdouble, (Cuint, Ptr{Cdouble}, Ptr{Cdouble}, Ptr{Cvoid}))
        chk(o, ccall(($(qsym("nlopt_add_", c, "_constraint")),libnlopt),
                   Result, (_Opt, Ptr{Cvoid}, Any, Cdouble),
                   o, nlopt_callback_wrapper_ptr,
                   getfield(o, :cb)[end], tol))
    end
end

function remove_constraints!(o::Opt)
    resize!(getfield(o, :cb), 1)
    chkn(ccall((:nlopt_remove_inequality_constraints,libnlopt),
               Result, (_Opt,), o))
    chkn(ccall((:nlopt_remove_equality_constraints,libnlopt),
               Result, (_Opt,), o))
end

############################################################################
# Vector-valued constraints

const empty_jac = Array{Cdouble}(undef,0,0) # for passing when grad == C_NULL

function nlopt_vcallback_wrapper(m::Cuint, res::Ptr{Cdouble},
                                 n::Cuint, x::Ptr{Cdouble},
                                 grad::Ptr{Cdouble}, d_::Ptr{Cvoid})
    d = unsafe_pointer_to_objref(d_)::Callback_Data
    try
        d.f(unsafe_wrap(Array, res, (convert(Int, m),)),
            unsafe_wrap(Array, x, (convert(Int, n),)),
            grad == C_NULL ? empty_jac
            : unsafe_wrap(Array, grad, (convert(Int, n),convert(Int, m))))
    catch e
        global nlopt_exception
        nlopt_exception = e
        force_stop!(d.o::Opt)
    end
    nothing
end

for c in (:inequality, :equality)
    cf = Symbol(string(c, "_constraint!"))
    @eval begin
        function $cf(o::Opt, f::Function, tol::Vector{Cdouble})
            push!(getfield(o, :cb), Callback_Data(f, o))
            nlopt_vcallback_wrapper_ptr = @cfunction(nlopt_vcallback_wrapper, Cvoid,
                  (Cuint, Ptr{Cdouble}, Cuint, Ptr{Cdouble}, Ptr{Cdouble}, Ptr{Cvoid}))
            chk(o, ccall(($(qsym("nlopt_add_", c, "_mconstraint")),
                        libnlopt),
                       Result, (_Opt, Cuint, Ptr{Cvoid}, Any, Ptr{Cdouble}),
                       o, length(tol), nlopt_vcallback_wrapper_ptr,
                       getfield(o, :cb)[end], tol))
        end
        $cf(o::Opt, f::Function, tol::AbstractVector{<:Real}) =
           $cf(o, f, Array{Float64}(tol))
        $cf(o::Opt, m::Integer, f::Function, tol::Real=0.0) =
           $cf(o, f, fill!(Cdouble(tol), m))
    end
end

############################################################################
# property-based getters setters opt.foo for Julia 0.7
# … at some point we will deprecate the old interface.

function Base.getproperty(o::Opt, p::Symbol)
    if p === :lower_bounds
        return lower_bounds(o)
    elseif p === :upper_bounds
        return upper_bounds(o)
    elseif p === :stopval
        return stopval(o)
    elseif p === :ftol_rel
        return ftol_rel(o)
    elseif p === :ftol_abs
        return ftol_abs(o)
    elseif p === :xtol_rel
        return xtol_rel(o)
    elseif p === :xtol_abs
        return xtol_abs(o)
    elseif p === :maxeval
        return maxeval(o)
    elseif p === :maxtime
        return maxtime(o)
    elseif p === :force_stop
        return force_stop(o)
    elseif p === :population
        return population(o)
    elseif p === :vector_storage
        return vector_storage(o)
    elseif p === :initial_step
        return initial_step(o)
    elseif p === :algorithm
        return algorithm(o)
    elseif p === :numevals
        return numevals(o)
    elseif p === :errmsg
        return errmsg(o)
    else
        error("type Opt has no readable property $p")
    end
end

function Base.setproperty!(o::Opt, p::Symbol, x)
    if p === :lower_bounds
        lower_bounds!(o, x)
    elseif p === :upper_bounds
        upper_bounds!(o, x)
    elseif p === :stopval
        stopval!(o, x)
    elseif p === :ftol_rel
        ftol_rel!(o, x)
    elseif p === :ftol_abs
        ftol_abs!(o, x)
    elseif p === :xtol_rel
        xtol_rel!(o, x)
    elseif p === :xtol_abs
        xtol_abs!(o, x)
    elseif p === :maxeval
        maxeval!(o, x)
    elseif p === :maxtime
        maxtime!(o, x)
    elseif p === :force_stop
        force_stop!(o, x)
    elseif p === :population
        population!(o, x)
    elseif p === :vector_storage
        vector_storage!(o, x)
    elseif p === :local_optimizer
        local_optimizer!(o, x)
    elseif p === :default_initial_step
        default_initial_step!(o, x)
    elseif p === :initial_step
        initial_step!(o, x)
    elseif p === :min_objective
        min_objective!(o, x)
    elseif p === :max_objective
        max_objective!(o, x)
    elseif p === :inequality_constraint
        inequality_constraint!(o, x)
    elseif p === :equality_constraint
        equality_constraint!(o, x)
    else
        error("type Opt has no writable property $p")
    end
    return x
end

Base.propertynames(o::Opt) =
   (:lower_bounds, :upper_bounds, :stopval, :ftol_rel, :ftol_abs, :xtol_rel, :xtol_abs, :maxeval, :maxtime, :force_stop, :population, :vector_storage, :initial_step, :algorithm, :local_optimizer, :default_initial_step, :initial_step, :min_objective, :max_objective, :inequality_constraint, :equality_constraint, :numevals, :errmsg)

############################################################################
# Perform the optimization:

function optimize!(o::Opt, x::Vector{Cdouble})
    if length(x) != ndims(o)
        throw(BoundsError())
    end
    opt_f = Array{Cdouble}(undef,1)
    ret = ccall((:nlopt_optimize,libnlopt), Result, (_Opt, Ptr{Cdouble},
                                                     Ptr{Cdouble}),
                o, x, opt_f)
    ret == INVALID_ARGS && chk(o, ret)
    return (opt_f[1], x, Symbol(ret))
end

optimize(o::Opt, x::AbstractVector{<:Real}) =
  optimize!(o, copyto!(Array{Cdouble}(undef,length(x)), x))

############################################################################

include("NLoptSolverInterface.jl")

end # module


q1 = nominal_configuration(hopper)
v1 = zeros(hopper.nq)

T = 100
h = 0.01

s = Simulator(hopper, T, h=h)
simulate!(s, q1, v1)

vis = Visualizer()
open(vis)
visualize!(vis, s)


export
    AbstractCustomNGraph,
    AbstractCustomNDiGraph,
    AbstractCustomNTree,
    AbstractCustomNETree,
    AbstractCustomTree,

    get_graph,
    get_vtx_ids,
    get_vtx_map,
    get_nodes,

    get_vtx,
    get_vtx_id,
    get_node,
    get_parent,

    get_edge,
    replace_edge!,
    # delete_edge!,
    # add_custom_edge!,

    node_val,
    edge_val,
    node_id, # may cause an issue
    edge_source,
    edge_target,

    set_vtx_map!,
    insert_to_vtx_map!,

    replace_node!,
    add_node!,
    make_node,
    make_edge,
    add_child!,
    add_parent!,
    rem_node!,
    rem_nodes!

abstract type AbstractCustomGraph <: AbstractGraph{Int} end

"""
    abstract type AbstractCustomNGraph{G,N,ID} <: AbstractCustomGraph

An abstract Custom Graph type, with an underlying graph of type `G`, nodes of
type `N` with ids of type `ID`. All concrete subtypes `CG<:AbstractCustomNGraph`
must implement the following methods: `get_graph(g::CG)`,`get_vtx_ids(g::CG)`,
`get_vtx_map(g::CG)`, and `get_nodes(g::CG)`. These methods are implemented by
default if `g::CG` has the following fields:
- `graph     ::CG`
- `nodes     ::Vector{N}`
- `vtx_map   ::Dict{ID,Int}`
- `vtx_ids   ::Vector{ID}`

Abstract subtypes of `AbstractCustomNGraph{G,N,ID}` include:
- `AbstractCustomNEGraph{G,N,E,ID}` - a graph with custom edges of type `E`
"""
abstract type AbstractCustomNGraph{G,N,ID} <: AbstractCustomGraph end

"""
    abstract type AbstractCustomNEGraph{G,N,E,ID} <: AbstractCustomNGraph{G,N,ID}

An abstract Custom Graph type, with an underlying graph of type `G`, nodes of
type `N` with ids of type `ID`, and edges of type `E`. All concrete subtypes
`CG<:AbstractCustomNEGraph` must implement the required `AbstractCustomNGraph`
interface in addition to the following methods:
- `out_edges(g::CG)` : returns an integer-indexed forward adjacency list `fadj`
    such that `fadj[u::Int][v::Int]` contains the custom edge associated
    with `u → v`.
- `in_edges(g::CG)` : returns an integer-indexed backward adjacency list `badj`
    such that `badj[v::Int][u::Int]` contains the custom edge associated
    with `u → v`.
The above methods are implemented by default if `g::CG` has the following
fields:
- `outedges  ::Vector{Dict{Int,E}}`
- `inedges   ::Vector{Dict{Int,E}}`
"""
abstract type AbstractCustomNEGraph{G,N,E,ID} <: AbstractCustomNGraph{G,N,ID} end

const AbstractCustomNDiGraph{N,ID} = AbstractCustomNGraph{DiGraph,N,ID}
const AbstractCustomNEDiGraph{N,E,ID} = AbstractCustomNEGraph{DiGraph,N,E,ID}

"""
    abstract type AbstractCustomNTree{N,ID} <: AbstractCustomNDiGraph{N,ID}

Abstract custom graph type with tree edge structure.
"""
abstract type AbstractCustomNTree{N,ID} <: AbstractCustomNDiGraph{N,ID} end
abstract type AbstractCustomNETree{N,E,ID} <: AbstractCustomNEDiGraph{N,E,ID} end
const AbstractCustomTree = Union{AbstractCustomNTree,AbstractCustomNETree}

# Common interface
_graph_type(::AbstractCustomNGraph{G,N,ID}) where {G,N,ID}  = G
_node_type(::AbstractCustomNGraph{G,N,ID}) where {G,N,ID}   = N
_id_type(::AbstractCustomNGraph{G,N,ID}) where {G,N,ID}     = ID
"""
    get_graph(g::AbstractCustomNGraph{G,N,ID})

return the underlying graph of type `G`.
"""
get_graph(g::AbstractCustomGraph)   = g.graph
get_graph(g::AbstractGraph)   = g
"""
    get_vtx_ids(g::AbstractCustomNGraph{G,N,ID})

return the vector `Vector{ID}` of `g`'s unique vertex ids.
"""
get_vtx_ids(g::AbstractCustomNGraph) = g.vtx_ids
"""
    get_vtx_map(g::AbstractCustomNGraph{G,N,ID})

return a data structure (e.g, a 'Dict{ID,Int}') mapping node id to node index.
"""
get_vtx_map(g::AbstractCustomNGraph) = g.vtx_map
"""
    get_nodes(g::AbstractCustomNGraph{G,N,ID})

Return the vector `Vector{N}` of `g`'s nodes.
"""
get_nodes(g::AbstractCustomNGraph)   = g.nodes

Base.zero(g::G) where {G<:AbstractCustomNGraph} = G(graph=_graph_type(g)())

get_vtx(g::AbstractCustomNGraph,v::Int) = v
get_vtx(g::AbstractCustomNGraph{G,N,ID},id::ID) where {G,N,ID} = get(get_vtx_map(g), id, -1)
get_vtx(g::AbstractCustomNGraph{G,N,ID},node::N) where {G,N,ID} = get_vtx(g,node.id)
get_vtx_id(g::AbstractCustomNGraph,v::Int)             = get_vtx_ids(g)[v]
get_node(g::AbstractCustomNGraph,v) = get_nodes(g)[get_vtx(g,v)]

for op in [:edgetype,:ne,:nv,:vertices,:edges,:is_cyclic,:topological_sort_by_dfs,:is_directed,:is_connected]
    @eval LightGraphs.$op(g::AbstractCustomNGraph) = $op(get_graph(g))
end
for op in [:outneighbors,:inneighbors,:indegree,:outdegree,:has_vertex]
    @eval LightGraphs.$op(g::AbstractCustomNGraph,v::Int) = $op(get_graph(g),v)
    @eval LightGraphs.$op(g::AbstractCustomNGraph,id) = $op(g,get_vtx(g,id))
end
for op in [:bfs_tree]
    @eval LightGraphs.$op(g::AbstractCustomNGraph,v::Int;kwargs...) = $op(get_graph(g),v;kwargs...)
    @eval LightGraphs.$op(g::AbstractCustomNGraph,id;kwargs...) = $op(get_graph(g),get_vtx(g,id);kwargs...)
end
for op in [:has_edge] #,:add_edge!,:rem_edge!]
    @eval LightGraphs.$op(s::AbstractCustomNGraph,u,v) = $op(get_graph(s),get_vtx(s,u),get_vtx(s,v))
end

"""
    node_id(node)

Return the id of a node. Part of the required node interface for nodes in an
 `AbstractCustomNGraph`.
"""
node_id(node)       = node.id

"""
    node_val(node)

Return the value associated with a node. Part of the optional node interface for
nodes in an `AbstractCustomNGraph`.
"""
node_val(node)      = node.val

"""
    edge_source(edge)

Returns the ID of the source node of an edge. Part of the required interface for
edges in an `AbstractCustomNEGraph`.
"""
edge_source(edge)   = edge.src
"""
    edge_target(edge)

Returns the ID of the target node of an edge. Part of the required interface for
edges in an `AbstractCustomNEGraph`.
"""
edge_target(edge)   = edge.dst
"""
    edge_val(edge)

Returns the value associated with an edge. Part of the optional interface for
edges in an `AbstractCustomNEGraph`.
"""
edge_val(edge)      = edge.val

function set_vtx_map!(g::AbstractCustomNGraph,node,id,v::Int)
    @assert nv(g) >= v
    get_vtx_map(g)[id] = v
    get_nodes(g)[v] = node
end
function insert_to_vtx_map!(g::AbstractCustomNGraph,node,id,idx::Int=nv(g))
    push!(get_vtx_ids(g), id)
    push!(get_nodes(g), node)
    set_vtx_map!(g,node,id,idx)
end

# TODO code a convenient macro to implement this
#
# for op in node_accessor_interface
#     @eval $op(g::AbstractCustomNGraph,v) = $op(get_node(g,v))
#     @eval $op(g::AbstractCustomNGraph) = map(v->$op(get_node(g,v)), vertices(g))
# end
# for op in node_mutator_interface
#     @eval $op(g::AbstractCustomNGraph,v,val) = $op(get_node(g,v),val)
#     @eval $op(g::AbstractCustomNGraph,val) = begin
#         for v in vertices(g)
#             $op(get_node(g,v),val)
#         end
#     end
# end

"""
    replace_node!(g::AbstractCustomNGraph{G,N,ID},node::N,id::ID) where {G,N,ID}

Replace the current node associated with `id` with the new node `node`.
"""
function replace_node!(g::AbstractCustomNGraph{G,N,ID},node::N,id::ID) where {G,N,ID}
    v = get_vtx(g, id)
    @assert v != -1 "node id $(string(id)) is not in graph and therefore cannot be replaced"
    set_vtx_map!(g,node,id,v)
    node
end

"""
    add_node!(g::AbstractCustomNGraph{G,N,ID},node::N,id::ID) where {G,N,ID}

Add `node` to `g` with associated `id`.
"""
function add_node!(g::AbstractCustomNGraph{G,N,ID},node::N,id::ID) where {G,N,ID}
    @assert !has_vertex(g, id) "Trying to add node $(string(node)) with id $(string(id)) to g, but a node with id $(string(id)) already exists in g"
    add_vertex!(get_graph(g))
    insert_to_vtx_map!(g,node,id,nv(g))
    add_edge_lists!(g)
    node
end
"""
    make_node(g::AbstractCustomNGraph{G,N,ID},val,id)

Construct a node of type `N` from val and id. This method must be implemented
for whatever custom node type is used.
"""
function make_node end
add_node!(g::AbstractCustomNGraph{G,N,ID},val,id::ID) where {G,N,ID} = add_node!(g,make_node(g,val,id),id)
# add_node!(g::AbstractCustomNEGraph{G,N,E,ID},val,id::ID) where {G,N,E,ID} = add_node!(g,make_node(g,val,id),id)
add_node!(g::AbstractCustomNGraph,val,id) = add_node!(g,make_node(g,val,id))
replace_node!(g::AbstractCustomNGraph{G,N,ID},val,id::ID) where {G,N,ID} = replace_node!(g,make_node(g,val,id),id)

"""
    function add_child!(graph,parent,node,id)

add node `child` to `graph` with id `id`, then add edge `parent` → `child`
"""
function add_child!(g::AbstractCustomNGraph{G,N,ID},parent,child,id) where {G,N,ID}
    n = add_node!(g,child,id)
    if add_edge!(g,parent,id)
        return n
    else
        rem_node!(g,id)
        return nothing
    end
end

"""
    function add_parent!(graph,child,parent,id)

add node `parent` to `graph` with id `id`, then add edge `parent` → `child`
"""
function add_parent!(g::AbstractCustomNGraph{G,N,ID},child,parent::N,id::ID) where {G,N,ID}
    n = add_node!(g,parent,id)
    if add_edge!(g,id,child)
        return n
    else
        rem_node!(g,id)
        return nothing
    end
end
for op in [:add_child!,:add_parent!]
    @eval $op(g::AbstractCustomNGraph{G,N,ID},u,v::N) where {G,N,ID} = $op(g,u,v,v.id)
end

"""
    swap_with_end_and_delete!(vec,v)

Replaces `vec[v]` with `last(vec)`, then removes the last element from `vec`
"""
function swap_with_end_and_delete!(vec::Vector,v)
    vec[v] = last(vec)
    pop!(vec)
    return vec
end

"""
    rem_node!

removes a node (by id) from g.
Note about LightGraphs.rem_vertex!:
"internally the removal is performed swapping the vertices `v` and `nv(G)``, and
removing the last vertex `nv(G)` from the graph"
"""
function rem_node!(g::AbstractCustomNGraph{G,N,ID}, id::ID) where {G,N,ID}
    v = get_vtx(g, id)
    rem_vertex!(get_graph(g), v)
    swap_with_end_and_delete!(get_nodes(g),v)
    swap_with_end_and_delete!(get_vtx_ids(g),v)
    # deleteat!(get_nodes(g), v)
    delete!(get_vtx_map(g), id)
    # if v was not at end, need to update id_map too.
    if v <= nv(g)
        get_vtx_map(g)[get_vtx_ids(g)[v]] = v
    end
    # deleteat!(get_vtx_ids(g), v)
    # @assert length(get_vtx_ids(g)) == length(get_vtx_map(g)) == nv(get_graph(g))
    # for vtx in v:nv(get_graph(g))
    #     n_id = get_vtx_ids(g)[vtx]
    #     get_vtx_map(g)[n_id] = vtx
    # end
    delete_from_edge_lists!(g,v) # no effect except for AbstractCustomNEGraph
    g
end
rem_node!(g::AbstractCustomNGraph, v) = rem_node!(g,get_vtx_id(g,v))
function rem_nodes!(g::AbstractCustomNGraph, vtxs::Vector)
    node_ids = collect(map(v->get_vtx_id(g,v), vtxs))
    for id in node_ids
        rem_node!(g,id)
    end
    g
end

# Edge graph interface
_edge_type(::AbstractCustomNEGraph{G,N,E,ID}) where {G,N,E,ID} = E

"""
    in_edges(g::AbstractCustomNEGraph{G,N,E,ID})

Returns an integer-indexed backward adjacency list `badj` (e.g.,
`badj::Vector{Dict{Int,E}}`) such that `badj[v::Int][u::Int]` contains the
custom edge associated with `u → v`.
"""
in_edges(g::AbstractCustomNEGraph) = g.inedges
in_edges(g::AbstractCustomNEGraph, v) = in_edges(g)[get_vtx(g,v)]
in_edge(g::AbstractCustomNEGraph, v, u) = in_edges(g,v)[get_vtx(g,u)]
function set_edge!(g::AbstractCustomNEGraph{G,N,E,ID}, u,v, edge::E) where {G,N,E,ID}
    out_edges(g,u)[get_vtx(g,v)] = edge
    in_edges(g,v)[get_vtx(g,u)] = edge
    # TODO for undirected graph, insert reversed edge too?
    edge
end

"""
    out_edges(g::AbstractCustomNEGraph{G,N,E,ID})

Returns an integer-indexed forward adjacency list `fadj` (e.g.,
`fadj::Vector{Dict{Int,E}}`) such that `fadj[u::Int][v::Int]` contains the
custom edge associated with `u → v`.
"""
out_edges(g::AbstractCustomNEGraph) = g.outedges
out_edges(g::AbstractCustomNEGraph, v) = out_edges(g)[get_vtx(g,v)]
out_edge(g::AbstractCustomNEGraph, u, v) = out_edges(g,u)[get_vtx(g,v)]
function get_edge(g::AbstractCustomNEGraph,u,v)
    @assert has_edge(g,u,v) "Edge $u → $v does not exist."
    return out_edge(g,u,v)
end
get_edge(g::AbstractCustomNEGraph,edge) = get_edge(g,edge_source(edge),edge_target(edge))
LightGraphs.has_edge(g::AbstractCustomNEGraph,e) = has_edge(g,edge_source(e),edge_target(e))

add_edge_lists!(g::AbstractCustomNGraph) = g
function add_edge_lists!(g::AbstractCustomNEGraph{G,N,E,ID}) where {G,N,E,ID}
    push!(g.outedges, Dict{Int,E}())
    push!(g.inedges, Dict{Int,E}())
    return g
end
delete_from_edge_lists!(g::AbstractCustomNGraph,v::Int) = g
function delete_from_edge_lists!(g::AbstractCustomNEGraph,v::Int)
    swap_with_end_and_delete!(in_edges(g),v)
    swap_with_end_and_delete!(out_edges(g),v)
    # deleteat!(in_edges(g),v)
    # deleteat!(out_edges(g),v)
    return g
end

# TODO Make a GraphType with default constructible edges
function add_custom_edge!(g::AbstractCustomNEGraph{G,N,E,ID},u,v,edge::E) where {G,N,E,ID}
    if !is_legal_edge(g,u,v,edge)
        @warn "An edge $u → $v is illegal in $g"
        return false
    end
    if add_edge!(get_graph(g),get_vtx(g,u),get_vtx(g,v))
        set_edge!(g,u,v,edge)
        return true
    end
    @warn "Cannot add edge $u → $v. Does an edge already exist?"
    # println("Cannot add edge $u → $v. Does an edge already exist?")
    return false
end
add_custom_edge!(g::AbstractCustomNEGraph,edge) = add_custom_edge!(g,edge_source(edge),edge_target(edge),edge)
# LightGraphs.add_edge!(g::AbstractCustomNEGraph,args...) = add_custom_edge!(g,args...)

add_custom_edge!(g::AbstractCustomNGraph,edge) = add_custom_edge!(g,edge_source(edge),edge_target(edge))
add_custom_edge!(g::AbstractCustomNGraph,u,v) = add_edge!(get_graph(g),get_vtx(g,u),get_vtx(g,v))
add_custom_edge!(g::AbstractCustomNGraph,u,v,args...) = add_custom_edge!(g,u,v)
# LightGraphs.add_edge!(g::AbstractCustomNGraph,u,v,edge) = add_custom_edge!(g,u,v) # no custom edge type here
# LightGraphs.add_edge!(g::AbstractCustomNGraph,u,v) = add_custom_edge!(g,u,v) # no custom edge type here
LightGraphs.add_edge!(g::AbstractCustomGraph,args...) = add_custom_edge!(g,args...) # no custom edge type here

"""
    make_edge(g::G,u,v,val) where {G}

Construct an edge `u → v` of type `_edge_type(g)` based on val. Default behavior
is to throw an error.
"""
function make_edge(g::G,u,v,val) where {G}
    throw(ErrorException(
    """
    MethodError: `make_edge(g,u,v,val)` not implemented.
    To add an edge to a graph g::$G, either:
    - add the edge explicitly using `add_edge!(g,edge,[u,v])
    - make a default constructor for the edge type
    - implement `GraphUtils.make_edge(g,u,v,val)`
    """
    ))
end
make_edge(g,u,v) = make_edge(g,u,v,nothing)
function add_custom_edge!(g::AbstractCustomNEGraph,u,v,val)
    add_custom_edge!(g,u,v,make_edge(g,u,v,val))
end
function add_custom_edge!(g::AbstractCustomNEGraph,u,v)
    add_custom_edge!(g,u,v,make_edge(g,u,v))
end
# function LightGraphs.add_edge!(g::AbstractCustomNEGraph,u,v,val)
#     add_edge!(g,u,v,make_edge(g,u,v,val))
# end
# function LightGraphs.add_edge!(g::AbstractCustomNEGraph,u,v)
#     add_edge!(g,u,v,make_edge(g,u,v))
# end
function replace_edge!(g::AbstractCustomNEGraph{G,N,E,ID},u,v,edge::E) where {G,N,E,ID}
    if has_edge(g,u,v)
        set_edge!(g,u,v,edge)
        return true
    end
    @warn "graph does not have edge $u → $v"
    return false
end
replace_edge!(g::AbstractCustomNEGraph,u,v,val) = replace_edge!(g,u,v,make_edge(g,u,v,val))
function replace_edge!(g::AbstractCustomNEGraph{G,N,E,ID},old_edge,edge::E) where {G,N,E,ID}
    replace_edge!(g,edge_source(old_edge),edge_target(old_edge),edge)
end

function delete_edge!(g::AbstractCustomNEGraph, u, v)
    if rem_edge!(get_graph(g),get_vtx(g,u),get_vtx(g,v))
        delete!(out_edges(g,u),get_vtx(g,v))
        delete!(in_edges(g,v),get_vtx(g,u))
        return true
    end
    @warn "Cannot remove edge $u → $v. Does it exist?"
    return false
end
delete_edge!(g::AbstractCustomNGraph, u, v) = rem_edge!(get_graph(g),get_vtx(g,u),get_vtx(g,v))
delete_edge!(g,edge) = delete_edge!(g,edge_source(edge),edge_target(edge))
LightGraphs.rem_edge!(g::AbstractCustomGraph, args...) = delete_edge!(g,args...)
# LightGraphs.rem_edge!(g::AbstractCustomNEGraph, u, v) = delete_edge!(g,u,v)
# LightGraphs.rem_edge!(g::AbstractCustomNEGraph, edge) = rem_edge!(g,edge_source(edge),edge_target(edge))

# Tree interface
get_parent(g::AbstractCustomTree,v) = get(inneighbors(g,v),1,-1)
is_legal_edge(g,u,v) = true
is_legal_edge(g,u,v,e) = is_legal_edge(g,u,v)
is_legal_edge(g::AbstractCustomTree,u,v) = !(has_vertex(g,get_parent(g,v)) || get_vtx(g,u) == get_vtx(g,v))
# function LightGraphs.add_edge!(g::AbstractCustomNTree,u,v)
function add_custom_edge!(g::AbstractCustomTree,u,v)
    is_legal_edge(g,u,v)
    if !is_legal_edge(g,u,v)
        return false
    end
    add_edge!(get_graph(g),get_vtx(g,u),get_vtx(g,v))
end

################################################################################
################################ Concrete Types ################################
################################################################################

export
    CustomNode,
    CustomEdge,
    CustomNGraph,
    CustomNDiGraph,
    CustomNEGraph,
    CustomNEDiGraph,
    CustomNTree,
    CustomNETree,
    NGraph,
    NEGraph,
    NTree,
    NETree

"""
    CustomGraph

An example concrete subtype of `AbstractCustomNGraph`.
"""
@with_kw struct CustomNGraph{G<:AbstractGraph,N,ID} <: AbstractCustomNGraph{G,N,ID}
    graph               ::G                     = G()
    nodes               ::Vector{N}             = Vector{N}()
    vtx_map             ::Dict{ID,Int}          = Dict{ID,Int}()
    vtx_ids             ::Vector{ID}            = Vector{ID}() # maps vertex uid to actual graph node
end
const CustomNDiGraph{N,ID} = CustomNGraph{DiGraph,N,ID}

"""
    CustomNEGraph{G,N,E,ID}

Custom graph type with custom edge and node types.
"""
@with_kw struct CustomNEGraph{G,N,E,ID} <: AbstractCustomNEGraph{G,N,E,ID}
    graph               ::G                     = G()
    nodes               ::Vector{N}             = Vector{N}()
    vtx_map             ::Dict{ID,Int}          = Dict{ID,Int}()
    vtx_ids             ::Vector{ID}            = Vector{ID}()
    inedges             ::Vector{Dict{Int,E}}   = Vector{Dict{Int,E}}()
    outedges            ::Vector{Dict{Int,E}}   = Vector{Dict{Int,E}}()
end
const CustomNEDiGraph{N,E,ID} = CustomNEGraph{N,E,ID}

"""
    CustomNTree

An example concrete subtype of `AbstractCustomNTree`.
"""
@with_kw struct CustomNTree{N,ID} <: AbstractCustomNTree{N,ID}
    graph               ::DiGraph               = DiGraph()
    nodes               ::Vector{N}             = Vector{N}()
    vtx_map             ::Dict{ID,Int}           = Dict{ID,Int}()
    vtx_ids             ::Vector{ID}             = Vector{ID}()
end

"""
    CustomNETree{G,N,E,ID}

Custom tree type with custom edge and node types.
"""
@with_kw struct CustomNETree{N,E,ID} <: AbstractCustomNETree{N,E,ID}
    graph               ::DiGraph               = DiGraph()
    nodes               ::Vector{N}             = Vector{N}()
    vtx_map             ::Dict{ID,Int}          = Dict{ID,Int}()
    vtx_ids             ::Vector{ID}            = Vector{ID}()
    inedges             ::Vector{Dict{Int,E}}   = Vector{Dict{Int,E}}()
    outedges            ::Vector{Dict{Int,E}}   = Vector{Dict{Int,E}}()
end

"""
    CustomNode{N,ID}

A custom node type. Fields:
- `id::ID`
- `val::N`
"""
struct CustomNode{N,ID}
    id::ID
    val::N
end
CustomNode{N,ID}(id::ID) where {N,ID} = CustomNode{N,ID}(id,N())
function make_node(g::AbstractCustomNGraph{G,CustomNode{N,ID},ID},val::N,id) where {G,N,ID}
    _node_type(g)(id,val)
end
# function make_node(g::AbstractCustomNEGraph{G,CustomNode{N,ID},E,ID},val::N,id) where {G,N,E,ID}
#     _node_type(g)(id,val)
# end
"""
    CustomEdge{E,ID}

A custom node type. Fields:
- `id::ID`
- `val::E`
"""
struct CustomEdge{E,ID}
    src::ID
    dst::ID
    val::E
end
CustomEdge{E,ID}(id1::ID,id2::ID) where {E,ID} = CustomEdge{E,ID}(id1,id2,E())
function make_edge(g::AbstractCustomNEGraph{G,N,CustomEdge{E,ID},ID},u,v,val::E) where {G,N,E,ID}
    _edge_type(g)(
        get_vtx_id(g,get_vtx(g,u)),
        get_vtx_id(g,get_vtx(g,v)),
        val)
end

# """
#     abstract type CustomNGraph{G,N,ID} <: AbstractCustomNGraph{G,CustomNode{N,ID},ID}
#
# Abstract custom graph with nodes of type `CustomNode{N,ID}`.
# """
# abstract type CustomNGraph{G,N,ID} <: AbstractCustomNGraph{G,CustomNode{N,ID},ID} end
# """
#     abstract type CustomNEGraph{G,N,ID} <: AbstractCustomNEGraph{G,CustomEdge{E,ID},CustomNode{N,ID},ID}
#
# Abstract custom graph with nodes of type `CustomNode{N,ID}` and edges of type
# `CustomEdge{E,ID}`.
# """
# abstract type CustomNEGraph{G,N,E,ID} <: AbstractCustomNEGraph{G,CustomEdge{E,ID},CustomNode{N,ID},ID} end
const NGraph{G,N,ID} = CustomNGraph{G,CustomNode{N,ID},ID}
const NEGraph{G,N,E,ID} = CustomNEGraph{G,CustomNode{N,ID},CustomEdge{E,ID},ID}
const NTree{N,ID} = CustomNTree{CustomNode{N,ID},ID}
const NETree{N,E,ID} = CustomNETree{CustomNode{N,ID},CustomEdge{E,ID},ID}

for T in (:CustomNGraph,:CustomNTree)
    @eval begin
        function Base.convert(::Type{X},g::AbstractCustomNGraph{G,N,ID}) where {G,N,ID,X<:$T}
            X(
                deepcopy(get_graph(g)),
                deepcopy(get_nodes(g)),
                deepcopy(get_vtx_map(g)),
                deepcopy(get_vtx_ids(g))
            )
        end
    end
end
for T in (:CustomNEGraph,:CustomNETree)
    @eval begin
        function Base.convert(::Type{X},g::AbstractCustomNEGraph{G,N,E,ID}) where {G,N,E,ID,X<:$T}
            X(
                deepcopy(get_graph(g)),
                deepcopy(get_nodes(g)),
                deepcopy(get_vtx_map(g)),
                deepcopy(get_vtx_ids(g)),
                deepcopy(in_edges(g)),
                deepcopy(out_edges(g)),
            )
        end
    end
end


export cnAdjLoadChkpt, cnAdjCalcdRdu, cnAdjDirect

"""
function cnAdjLoadChkpt:
  loads a checkpoint from disk based on desired forward sweep index

  returns eqn object, which has only the necessary correct fields for the unsteady adjoint
"""
# function cnAdjLoadChkpt(mesh, sbp, opts, adj, physics_func, i_fwd, t)
function cnAdjLoadChkpt(mesh, sbp, opts, adj, physics_func, i_fwd)

  # initialize dummy eqn object for jacobian calculation use
  # Note: can't just call this:
  #   eqn_dummy = AdvectionData_{Tsol, Tres, Tdim, Tmsh}(mesh, sbp, opts)
  # because it needs AdvectionEquationMod loaded, which needs PDESolver loaded, which can't be loaded more than once.
  eqn_dummy = eqn_deepcopy(mesh, sbp, adj, opts)                     # allocate a dummy eqn object

  # println("in cnAdjLoadChkpt: pointer(adj.q_vec): ", pointer(adj.q_vec))
  # println("in cnAdjLoadChkpt: pointer(adj.res_vec): ", pointer(adj.res_vec))
  # println("in cnAdjLoadChkpt: pointer(eqn_dummy.q_vec):   ", pointer(eqn_dummy.q_vec))
  # println("in cnAdjLoadChkpt: pointer(eqn_dummy.res_vec): ", pointer(eqn_dummy.res_vec))

  qvec_filename = string("qvec_for_adj-", i_fwd, ".dat")
  println("  Calculating Jac using forward sweep data from: ", qvec_filename)
  q_vec_with_complex = readdlm(qvec_filename)
  eqn_dummy.q_vec = q_vec_with_complex[:,1]     # because readdlm gives a field to the zero-valued complex part
  array1DTo3D(mesh, sbp, eqn_dummy, opts, eqn_dummy.q_vec, eqn_dummy.q)

  check_q_qvec_consistency(mesh, sbp, eqn_dummy, opts)
  println("  eqn_dummy.q_vec loaded. i_fwd: ", i_fwd)
  # print_qvec_coords(mesh, sbp, eqn_dummy, opts)

  vis_filename = string("solution_loadedfromdisk_ifwd-", i_fwd)
  saveSolutionToMesh(mesh, real(eqn_dummy.q_vec))
  writeVisFiles(mesh, vis_filename)

  # sync up eqn_dummy.q and eqn_dummy.q_vec

  # TODO: is this needed here? YES.
  #     explain why here
  # TODO new: I don't think so now that eqn_deepcopy is properly implemented
  # eqn_dummy.q = reshape(eqn_dummy.q_vec, mesh.numDofPerNode, mesh.numNodesPerElement, mesh.numEl)
  # eqn_dummy.res = reshape(eqn_dummy.res_vec, mesh.numDofPerNode, mesh.numNodesPerElement, mesh.numEl)

  #=
  # make eqn object consistent
  evalResidual(mesh, sbp, eqn_dummy, opts)
  array3DTo1D(mesh, sbp, eqn_dummy, opts, eqn_dummy.res, eqn_dummy.res_vec)

  # TODO: fix ordering & repetition of these
  eqn_dummy.q = reshape(eqn_dummy.q_vec, mesh.numDofPerNode, mesh.numNodesPerElement, mesh.numEl)
  eqn_dummy.res = reshape(eqn_dummy.res_vec, mesh.numDofPerNode, mesh.numNodesPerElement, mesh.numEl)
  =#
  # looks like the above commented section doesn't make a difference

  # use startDataExchange to sync up q/q_vec and q_face_send/recv
  if opts["parallel_type"] == 2 && mesh.npeers > 0
    startDataExchange(mesh, opts, eqn_dummy.q, eqn_dummy.q_face_send, eqn_dummy.q_face_recv, eqn_dummy.params.f)
  end

  return eqn_dummy

end   # end function cnAdjLoadChkpt

"""
function cnAdjCalcdRdu:
  calculates the Jacobian dRdu at an arbitrary time t given an associated eqn_dummy object

  returns a jac
"""
function cnAdjCalcdRdu(mesh, sbp, opts, eqn_dummy, physics_func, i_fwd, t)
  # Note: probably don't need to allocate another jac, but this should cause no problems aside from allocation cost
  newton_data_discard, jac, rhs_vec_discard = setupNewton(mesh, mesh, sbp, eqn_dummy, opts, physics_func, () )

  # make sure we're doing complex step! since res_copy is zeros, it would mess up the FD calc
  assert(opts["jac_method"] == 2)
  epsilon = opts["epsilon"]
  pert = complex(0, epsilon)

  # checked: eqn_dummy is loaded properly
  # checked: cjc args are correct
  # checked: norm of eqn_dummy.q_vec in cJC matches it here

  println("  calculating jac for loaded eqn_dummy. i_fwd = ", i_fwd,", t = ", t)
  calcJacobianComplex(newton_data_discard, mesh, sbp, eqn_dummy, opts, physics_func, pert, jac, t)

  println(" ::::::::::::: in cnAdjCalcdRdu, after calcJacobianComplex, norm(jac): ", norm(jac, 1))

  # filename = string("jac_from_cnAdjCalcdRdu-i_fwd-",i_fwd,".dat")
  # writedlm(filename, jac)

  return jac
end   # end function cnAdjCalcdRdu

"""
function cnAdjDirect:
  performs a direct solve of the linear adjoint PDE

  (*)_(i+1) corresponds to t
  (*)_i corresponds to t_nextstep

  given psi_(i+1)

  returns psi_i
"""
function cnAdjDirect(mesh, sbp, opts, adj, physics_func, jac, i_fwd, h, t_steps, t, dRdu_global_rev)
  # Note: t is passed in (instead of t_nextstep), but t_nextstep is calc'd & used below
  #
  # Note: adj is not altered in this function. It is only used for passing into cnAdjLoadChkpt for an eqn_deepcopy

# adj_nextstep.q_vec = cnAdjDirect(mesh, sbp, opts, adj_nextstep, jac, i_fwd, t_nextstep)

  t_nextstep = t - h
  t_nextstep = negativeZeroCheck(t_nextstep)   # ensure negative zero is changed to zero

  # Load checkpoint at time t, aka i+1
  eqn_dummy = cnAdjLoadChkpt(mesh, sbp, opts, adj, physics_func, i_fwd)   # adj is only used for a eqn_deepcopy within
                                                                          #   cnAdjLoadCheckpt
  # checked: eqn_dummy is loaded properly
  # Use time t's eqn.q_vec and t_nextstep, aka i, to compute the jac (dRdu) at t_nextstep aka i
  jac = cnAdjCalcdRdu(mesh, sbp, opts, eqn_dummy, physics_func, i_fwd, t_nextstep)
  dRdu_i = transpose(jac)       # see derivation for transpose justification

  # TODO: double check that there is not an off by one error on dRdu:
  #       even though calculated at t_nextstep, am I loading q_vec at i+1 instead of i?

  I = eye(length(adj.q_vec))

  if opts["uadj_global"]
    ### GLOBAL dRdu section
    B1_pos_for_dRdu_global =  1.0*I - 0.5*h*dRdu_i
    B1_neg_for_dRdu_global = -1.0*I - 0.5*h*dRdu_i

    blksz = mesh.numDof
    # blksz = 3   # testing 44
    # B1_pos_for_dRdu_global = ones(blksz, blksz)*44
    # B1_neg_for_dRdu_global = ones(blksz, blksz)*55

    println(" GLOBAL: forming global dRdu (rev), i_fwd = $i_fwd")
    println(" GLOBAL: size of B1_pos_for_dRdu_global: ", size(B1_pos_for_dRdu_global))
    row_ix_start = (i_fwd-2)*blksz + 1
    row_ix_end = (i_fwd-2)*blksz + blksz
    col_ix_start = (i_fwd-2)*blksz + 1
    col_ix_end = (i_fwd-2)*blksz + blksz
    println(" GLOBAL: diagonal block ix's: [",row_ix_start,":",row_ix_end,", ",col_ix_start,":", col_ix_end,"]")
    dRdu_global_rev[row_ix_start:row_ix_end, col_ix_start:col_ix_end] = B1_pos_for_dRdu_global

    if i_fwd != t_steps + 1
      col_ix_start = (i_fwd-2)*blksz + blksz + 1
      col_ix_end = (i_fwd-2)*blksz + 2*blksz
      row_ix_start = (i_fwd-2)*blksz + 1
      row_ix_end = (i_fwd-2)*blksz + blksz
      println(" GLOBAL: off-diagonal block ix's: [",row_ix_start,":",row_ix_end,", ",col_ix_start,":", col_ix_end,"]")

      dRdu_global_rev[row_ix_start:row_ix_end, col_ix_start:col_ix_end] = B1_neg_for_dRdu_global
    end   # end of "if i_fwd != t_steps + 1"
    ### end GLOBAL
  end # end of "if opts["uadj_global"]

  dJdu_i = calcdJdu_CS(mesh, sbp, eqn_dummy, opts, h, t)  # obtain dJdu at time step i

  # 20170831. Looks like there's a transpose missing here on B1.
  # See notes, LHS, 08/30/2017, for rigorous explanation.
  # B1 = I - 0.5*h*dRdu_i
  # B2 = adj.q_vec + 0.5*h*dRdu_i*adj.q_vec - dJdu_i
  B1 = transpose(I - 0.5*h*dRdu_i)
  B2 = adj.q_vec + 0.5*h*dRdu_i*adj.q_vec - dJdu_i

  nextstep_q_vec = zeros(adj.q_vec)
  nextstep_q_vec = B1\B2

  # filename = string("eqn_dummyqvec_from_cnAdjDirect-i_fwd-",i_fwd,".dat")
  # writedlm(filename, eqn_dummy.q_vec)

  return nextstep_q_vec, jac

end     # end function cnAdjDirect


using IncrementalInference
using Test

# setSerializationNamespace!("Main" => Main)

@testset "test solve by saving and loading basic jld..." begin

fgt = initfg()

addVariable!(fgt, :x1, ContinuousScalar)
addFactor!(fgt, [:x1], Prior(Normal()))

addVariable!(fgt, :x2, ContinuousScalar)
addFactor!(fgt, [:x1;:x2], LinearConditional(Normal(10,1)))

savejld(fgt)
# savejld(fgt, file=joinpath(Pkg.dir("IncrementalInference"),"test","testdata","compatibility_test_fg.jld"))
fgl, = loadjld()

fct = solverData(getFactor(fgt, :x1x2f1)).fnc.cpt[1].factormetadata
fcl = solverData(getFactor(fgl, :x1x2f1)).fnc.cpt[1].factormetadata

@test fct.solvefor == fcl.solvefor
@test length(fct.variableuserdata) == length(fcl.variableuserdata)

batchSolve!(fgt)
batchSolve!(fgl)

# test again after solve
@test fct.solvefor == fcl.solvefor
@test length(fct.variableuserdata) == length(fcl.variableuserdata)

# save load and repeat tests
savejld(fgt, file="tempfg.jld2")
fgl, = loadjld(file="tempfg.jld2")

Base.rm("tempfg.jld2")

fct = solverData(getFactor(fgt, :x1x2f1)).fnc.cpt[1].factormetadata
fcl = solverData(getFactor(fgl, :x1x2f1)).fnc.cpt[1].factormetadata

# @test fct.solvefor == fcl.solvefor # defaults to :null during load
@test length(fct.variableuserdata) == length(fcl.variableuserdata)

end

@testset "test backwards compatibility of loadjld from previous versions of IIF (from an existing test data file)..." begin

filename = joinpath(dirname(pathof(IncrementalInference)), "..", "test","testdata","compatibility_test_fg.jld2")
# savejld(fgt, file=filename)  # when changing to a new compat test file (human required)
fgprev, = loadjld(file=filename )

fc = solverData(getFactor(fgprev, :x1x2f1))
@test length(fc.fnc.cpt[1].factormetadata.variableuserdata) == 2
@test fc.fnc.cpt[1].factormetadata.solvefor == :null

# batchSolve!(fgprev)
solveTree!(fgprev)

end

#


module FastAI

using Reexport

@reexport using DLPipelines
@reexport using FluxTraining
@reexport using DataLoaders
@reexport using Flux

using Animations
using AbstractPlotting
using Colors
using DataAugmentation
using DataAugmentation: getbounds, makebounds
using DLPipelines: methoddataset, methodmodel, methodlossfn, methoddataloaders
using LearnBase: getobs, nobs
using FilePathsBase
using FixedPointNumbers
using Flux
using FluxTraining: Learner, handle
using FluxTraining.Events
using MLDataPattern
using Parameters
using StaticArrays
using WordTokenizers: spaces, TokenBuffer, character, isdone, flush!

include("tasks.jl")
include("plotting.jl")
include("learner.jl")

# method implementations and helpers
include("./steps/utils.jl")
include("./steps/step.jl")
include("./steps/spatial.jl")
include("./steps/imagepreprocessing.jl")
include("./methods/imageclassification.jl")
include("./methods/imagesegmentation.jl")

# submodules
include("datasets/Datasets.jl")
using .Datasets

include("models/Models.jl")
using .Models

# training
include("training/utils.jl")
include("training/onecycle.jl")
include("training/finetune.jl")
include("training/lrfind.jl")

export methodlossfn

export
    # submodules
    Datasets,
    Models,
    datasetpath,
    loadtaskdata,
    mapobs,
    groupobs,
    filterobs,
    shuffleobs,

    # method API
    methodmodel,
    methoddataset,
    methoddataloaders,
    methodlossfn,
    getobs,
    nobs,

    # pipeline steps
    ProjectiveTransforms, ImagePreprocessing,

    # tasks
    ImageClassificationTask,
    ImageSegmentationTask,

    # methods
    ImageClassification,
    ImageSegmentation,

    # training
    methodlearner,
    Learner,
    fit!,
    fitonecycle!,
    finetune!,

    gpu

end  # module


module DragWrapper

using JavaCall

const AbsoluteDate = @jimport org.orekit.time.AbsoluteDate
const AbstractDragForceModel = @jimport org.orekit.forces.drag.AbstractDragForceModel
const AbstractForceModel = @jimport org.orekit.forces.AbstractForceModel
const Atmosphere = @jimport org.orekit.models.earth.atmosphere.Atmosphere
const Class = @jimport java.lang.Class
const DragForce = @jimport org.orekit.forces.drag.DragForce
const DragSensitive = @jimport org.orekit.forces.drag.DragSensitive
const Field = @jimport org.hipparchus.Field
const FieldAbsoluteDate = @jimport org.orekit.time.FieldAbsoluteDate
const FieldRotation = @jimport org.hipparchus.geometry.euclidean.threed.FieldRotation
const FieldSpacecraftState = @jimport org.orekit.propagation.FieldSpacecraftState
const FieldTimeDerivativesEquations = @jimport org.orekit.propagation.numerical.FieldTimeDerivativesEquations
const FieldVector3D = @jimport org.hipparchus.geometry.euclidean.threed.FieldVector3D
const ForceModel = @jimport org.orekit.forces.ForceModel
const Frame = @jimport org.orekit.frames.Frame
const IsotropicDrag = @jimport org.orekit.forces.drag.IsotropicDrag
const JString = @jimport java.lang.String
const NavigableSet = @jimport java.util.NavigableSet
const Object = @jimport java.lang.Object
const ParameterDriver = @jimport org.orekit.utils.ParameterDriver
const RealFieldElement = @jimport org.hipparchus.RealFieldElement
const Rotation = @jimport org.hipparchus.geometry.euclidean.threed.Rotation
const SpacecraftState = @jimport org.orekit.propagation.SpacecraftState
const Stream = @jimport java.util.stream.Stream
const TimeDerivativesEquations = @jimport org.orekit.propagation.numerical.TimeDerivativesEquations
const TimeScale = @jimport org.orekit.time.TimeScale
const TimeSpanDragForce = @jimport org.orekit.forces.drag.TimeSpanDragForce
const TimeSpanMap = @jimport org.orekit.utils.TimeSpanMap
const TimeSpanMap_Span = @jimport org.orekit.utils.TimeSpanMap$Span
const Vector3D = @jimport org.hipparchus.geometry.euclidean.threed.Vector3D

export AbstractDragForceModel
export DragForce
export DragSensitive
export IsotropicDrag
export TimeSpanDragForce
export acceleration
export add_contribution
export add_drag_sensitive_valid_after
export add_drag_sensitive_valid_before
export depends_on_position_only
export drag_acceleration
export equals
export extract_drag_sensitive_range
export extract_parameters
export get_atmosphere
export get_class
export get_drag_parameters_drivers
export get_drag_sensitive
export get_drag_sensitive_span
export get_events_detectors
export get_field_events_detectors
export get_parameter_driver
export get_parameters
export get_parameters_drivers
export get_spacecraft
export get_transitions
export hash_code
export init
export is_supported
export notify
export notify_all
export to_string
export wait

include("abstract_drag_force_model.jl")
include("drag_force.jl")
include("drag_sensitive.jl")
include("isotropic_drag.jl")
include("time_span_drag_force.jl")

end


# 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
include("halfedge.jl")

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

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

function add_edge_face(edge_faces, A, B, left_face_index)
    #println("add_edge_face ", A, " ", B, " ", left_face_index)
    edge_key = (A,B)
    @assert !haskey(edge_faces, edge_key)
    edge_faces[edge_key] = left_face_index
end

const Edge = Tuple{Int32,Int32}
const Face = Int32

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)

    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 = [1:4:output_num_quads*4;]
    output_loop_total = fill(UInt32(4), output_num_quads)
    output_loops = Array{UInt32}(undef, 4*output_num_quads)

    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 = Vector{Float32}([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

        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]

        edge_point = (
            get_vertex(output_vertices, face_point_index(he.face))
            +
            get_vertex(output_vertices, he.source)
            +
            get_vertex(output_vertices, he.target)
        )
        n = 3

        if he.sibling != nothing
            he2 = he.sibling
            edge_point += get_vertex(output_vertices, face_point_index(he2.face))
            n += 1
        end

        set_vertex(output_vertices, edge_point_index(ei), edge_point / n)
    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)
        F_sum = Vector{Float32}([0, 0, 0])
        R_sum = Vector{Float32}([0, 0, 0])
        n = 0

        he = start = vertex_start_edges[vi]
        while true
            F_sum += get_vertex(output_vertices, face_point_index(he.face))
            # XXX could take out P here as use it once in R= below
            R_sum += 0.5*(P + get_vertex(output_vertices, he.target))
            n += 1

            # XXX need to recheck this works correctly
            if he.sibling == nothing break end

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

        F = F_sum / n
        R = R_sum / n

        set_vertex(output_vertices, vi, (F + 2*R + (n-3)*P) / n)
    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