import JLD
import ..Infer
import ..ParallelRun: infer_init, one_node_infer, BoundingBox,
                      one_node_joint_infer, one_node_single_infer

"""
compare_vp_params
Helper to check whether two sets of variational parameters from the
result of joint inference are the same.

Return true if vp params are the same, false otherwise
"""
function compare_vp_params(r1, r2)

    # Create a map from thingid -> vp for r1
    r1_vp = Dict{Int64, Vector{Float64}}()
    for r1_result in r1
        r1_vp[r1_result.thingid] = r1_result.vs
    end

    # Check the existence and equivalence of each source's vp in r2
    for r2_result in r2
        if !haskey(r1_vp, r2_result.thingid) || r1_vp[r2_result.thingid] != r2_result.vs
            if r1_vp[r2_result.thingid] != r2_result.vs
                println("compare_vp_params: Mismatch - $(r1_vp[r2_result.thingid]) vs $(r2_result.vs)")
            end
            return false
        end
    end

    return length(r1) == length(r2)
end

"""
compute_obj_value
computes obj value given set of results from one_node_infer and one_node_joint_infer
"""
function compute_obj_value(results,
                           rcfs::Vector{RunCamcolField},
                           stagedir::String;
                           box=BoundingBox(-1000.,1000.,-1000.,1000.))
    catalog, target_sources = infer_init(rcfs, stagedir; box=box)
    images = SDSSIO.load_field_images(rcfs, stagedir)

    vp = [r.vs for r in results]

    # it may be better to pass `patches` as an argument to `compute_obj_value`.
    patches = Infer.get_sky_patches(images, catalog[target_sources])

    # if we don't call `load_active_pixels!`, these patches will be different
    # than the patches we used for optimizing---and then the objective
    # function could also be slightly different
    Infer.load_active_pixels!(images, patches)

    # this works since we're just generating patches for active_sources.
    # if instead you pass patches for all sources, then instead we'd used
    # active_sources = target_sources
    active_sources = collect(1:length(target_sources))

    ea = ElboArgs(images, vp, patches, active_sources,
                  calculate_gradient=false, calculate_hessian=false)
    DeterministicVI.elbo(ea).v[]
end

"""
test_different_result_with_different_iter()
Using 3 iters instead of 1 iters should result in a different set of parameters.
"""
function test_different_result_with_different_iter()
    # This bounding box has overlapping stars. (neighbor map is not empty)
    box = BoundingBox(4.39, 164.41, 9.11, 39.13)
    field_triplets = [RunCamcolField(3900, 6, 269),]

    infer_few(ctni...) = one_node_joint_infer(ctni...;
                                              n_iters=1,
                                              within_batch_shuffling=false)
    result_iter_1 = one_node_infer(field_triplets, datadir;
                                   infer_callback=infer_few, box=box)

    infer_many(ctni...) = one_node_joint_infer(ctni...;
                                             n_iters=5,
                                             within_batch_shuffling=false)
    result_iter_5 = one_node_infer(field_triplets, datadir;
                                   infer_callback=infer_many, box=box)

    # Make sure that parameters are not the same
    @test !compare_vp_params(result_iter_1, result_iter_5)
end

"""
test_same_result_with_diff_batch_sizes
Varying batch sizes using the cyclades algorithm should not change final objective value.
"""
function test_same_result_with_diff_batch_sizes()
    # This bounding box has overlapping stars. (neighbor map is not empty)
    box = BoundingBox(4.39, 164.41, 9.11, 39.13)
    field_triplets = [RunCamcolField(3900, 6, 269),]

    # With batch size = 7
    infer_few(ctni...) = one_node_joint_infer(ctni...;
                                n_iters=3,
                                batch_size=7,
                                within_batch_shuffling=false)
    result_bs_7 = one_node_infer(field_triplets, datadir;
                                 infer_callback=infer_few,
                                 box=box)

    # With batch size = 39
    infer_many(ctni...) = one_node_joint_infer(ctni...;
                                n_iters=3,
                                batch_size=39,
                                within_batch_shuffling=false)
    result_bs_39 = one_node_infer(field_triplets, datadir;
                                  infer_callback=infer_many,
                                  box=box)

    # Make sure that parameters are exactly the same
    @test compare_vp_params(result_bs_7, result_bs_39)
end

"""
test infer multi iter with a single (run, camcol, field).
This is basically just to make sure it runs at all.
"""
function test_one_node_joint_infer()
    # very small patch of sky that turns out to have 4 sources.
    # We checked that this patch is in the given field.
    box = BoundingBox(164.39, 164.41, 39.11, 39.13)
    field_triplets = [RunCamcolField(3900, 6, 269),]
    result = one_node_infer(field_triplets, datadir;
                            infer_callback=one_node_joint_infer,
                            box=box)
end

"""
test infer multi iter obj overlapping.
This makes sure one_node_joint_infer achieves sum objective value less
than single iter on non overlapping sources.
"""
function test_one_node_joint_infer_obj_overlapping()

    # This bounding box has overlapping stars. (neighbor map is not empty)
    box = BoundingBox(164.39, 164.41, 39.11, 39.13)
    field_triplets = [RunCamcolField(3900, 6, 269),]

    # 100 iterations
    tic()
    infer_multi(ctni...) = one_node_joint_infer(ctni...;
                                     n_iters=100,
                                     within_batch_shuffling=false)
    result_multi = one_node_infer(field_triplets, datadir;
                                  infer_callback=infer_multi,
                                  box=box)
    multi_iter_time = toq()
    score_multi = compute_obj_value(result_multi, field_triplets, datadir; box=box)

    # 2 iterations
    tic()
    infer_two(ctni...) = one_node_joint_infer(ctni...;
                                     n_iters=2,
                                     within_batch_shuffling=false)
    result_two = one_node_infer(field_triplets, datadir;
                                  infer_callback=infer_two,
                                  box=box)
    two_iter_time = toq()
    score_two = compute_obj_value(result_two, field_triplets, datadir; box=box)

    # One node infer (1 iteration, butm ore newton steps)
    tic()
    result_single = one_node_infer(field_triplets, datadir; box=box)
    single_iter_time = toq()
    score_single = compute_obj_value(result_single, field_triplets, datadir; box=box)

    println("One node joint infer objective value: $(score_multi)")
    println("One node joint infer 2 iter objective value: $(score_two)")
    println("Single iter objective value: $(score_single)")
    println("One node joint infer time: $(multi_iter_time)")
    println("One node joint infer 2 iter time: $(two_iter_time)")
    println("Single iter time: $(single_iter_time)")
    @test score_multi > score_single
    @test score_multi > score_two
end

"""
helper edges_between_sources
Helper function to determine if there are edges between 2 sets of sources
"""
function edges_between_sources(a, b, neighbor_map)
    for v_a in a, v_b in b
        # We assume undirected graph (symmetric map)
        if v_b in neighbor_map[v_a]
            println("Found edge between elements $(v_a) and $(v_b)")
            return true
        end
    end
    return false
end

"""
test cyclades partitioning.
Makes sure cyclades partitioning correctly partitions sources
"""
function test_cyclades_partitioning()
    """
    The conflict pattern is a connected 4x4 grid:
    1 -  2 - 3  - 4
    |    |   |    |
    5 -  6 - 7  - 8
    |    |   |    |
    9 - 10 - 11 - 12
    |    |   |    |
   13 - 14 - 15 - 16

    We offset 5 from every value.
    """
    neighbor_map = Dict(6=>[7,10],
                        7=>[6,8,14],
                        8=>[7,9,12],
                        9=>[8,13],
                        10=>[6,11,14],
                        11=>[7,10,12,15],
                        12=>[8,11,13,16],
                        13=>[9,12,17],
                        14=>[11,15,18],
                        15=>[11,14,16,19],
                        16=>[12,15,17,20],
                        17=>[13,16,21],
                        18=>[14,19],
                        19=>[18,15,20],
                        20=>[16,19,21],
                        21=>[17,20])

    target_sources = [6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21]

    # 3 threads, 16 sources, batch_size = 4
    source_assignment = ParallelRun.partition_cyclades(3,
                                            target_sources,
                                            neighbor_map,
                                            batch_size=4)

    # Gather the entities across all batches.
    # 1. They should sum up to 16 exactly.
    # 2. They should be numbered from 1 - 16.
    all_sources =  Vector{Int64}()
    n_simulated_threads = length(source_assignment)
    n_batches = length(source_assignment[1])
    @test n_simulated_threads == 3
    for simulated_thread_id=1:3
        # Every thread must have the same # of batches.
        @test n_batches == length(source_assignment[simulated_thread_id])
        for batch=1:n_batches
            for source_id in source_assignment[simulated_thread_id][batch]
                push!(all_sources, source_id)
            end
        end
    end
    @test length(all_sources) == 16
    for source_id=1:16
        @test source_id in all_sources
    end

    # Now make sure elements of the same batches between threads do not
    # conflict (have an edge).
    for batch=1:n_batches
        t1_sources = [target_sources[x] for x in source_assignment[1][batch]]
        t2_sources = [target_sources[x] for x in source_assignment[2][batch]]
        t3_sources = [target_sources[x] for x in source_assignment[3][batch]]
        println("Batch $(batch) assignments: $t1_sources $t2_sources $t3_sources")
        @test !edges_between_sources(t1_sources, t2_sources, neighbor_map)
        @test !edges_between_sources(t1_sources, t3_sources, neighbor_map)
        @test !edges_between_sources(t2_sources, t3_sources, neighbor_map)
    end

    println("Cyclades partitioning test succeeded")
end

test_different_result_with_different_iter()
test_same_result_with_diff_batch_sizes()
test_one_node_joint_infer_obj_overlapping()

# Run this multiple times, since the cyclades algorithm shuffles the elements
# before batching them up.
for i=1:20
    test_cyclades_partitioning()
end

test_one_node_joint_infer()


# Julia wrapper for header: /usr/include/scip/cons_soc.h
# Automatically generated using Clang.jl wrap_c

function SCIPincludeConshdlrSOC(scip)
    ccall((:SCIPincludeConshdlrSOC, libscip), SCIP_RETCODE, (Ptr{SCIP_},), scip)
end

function SCIPcreateConsSOC(scip, cons, name, nvars, vars, coefs, offsets, constant, rhsvar, rhscoeff, rhsoffset, initial, separate, enforce, check, propagate, _local, modifiable, dynamic, removable)
    ccall((:SCIPcreateConsSOC, libscip), SCIP_RETCODE, (Ptr{SCIP_}, Ptr{Ptr{SCIP_CONS}}, Cstring, Cint, Ptr{Ptr{SCIP_VAR}}, Ptr{Cdouble}, Ptr{Cdouble}, Cdouble, Ptr{SCIP_VAR}, Cdouble, Cdouble, UInt32, UInt32, UInt32, UInt32, UInt32, UInt32, UInt32, UInt32, UInt32), scip, cons, name, nvars, vars, coefs, offsets, constant, rhsvar, rhscoeff, rhsoffset, initial, separate, enforce, check, propagate, _local, modifiable, dynamic, removable)
end

function SCIPcreateConsBasicSOC(scip, cons, name, nvars, vars, coefs, offsets, constant, rhsvar, rhscoeff, rhsoffset)
    ccall((:SCIPcreateConsBasicSOC, libscip), SCIP_RETCODE, (Ptr{SCIP_}, Ptr{Ptr{SCIP_CONS}}, Cstring, Cint, Ptr{Ptr{SCIP_VAR}}, Ptr{Cdouble}, Ptr{Cdouble}, Cdouble, Ptr{SCIP_VAR}, Cdouble, Cdouble), scip, cons, name, nvars, vars, coefs, offsets, constant, rhsvar, rhscoeff, rhsoffset)
end

function SCIPgetNlRowSOC(scip, cons, nlrow)
    ccall((:SCIPgetNlRowSOC, libscip), SCIP_RETCODE, (Ptr{SCIP_}, Ptr{SCIP_CONS}, Ptr{Ptr{SCIP_NLROW}}), scip, cons, nlrow)
end

function SCIPgetNLhsVarsSOC(scip, cons)
    ccall((:SCIPgetNLhsVarsSOC, libscip), Cint, (Ptr{SCIP_}, Ptr{SCIP_CONS}), scip, cons)
end

function SCIPgetLhsVarsSOC(scip, cons)
    ccall((:SCIPgetLhsVarsSOC, libscip), Ptr{Ptr{SCIP_VAR}}, (Ptr{SCIP_}, Ptr{SCIP_CONS}), scip, cons)
end

function SCIPgetLhsCoefsSOC(scip, cons)
    ccall((:SCIPgetLhsCoefsSOC, libscip), Ptr{Cdouble}, (Ptr{SCIP_}, Ptr{SCIP_CONS}), scip, cons)
end

function SCIPgetLhsOffsetsSOC(scip, cons)
    ccall((:SCIPgetLhsOffsetsSOC, libscip), Ptr{Cdouble}, (Ptr{SCIP_}, Ptr{SCIP_CONS}), scip, cons)
end

function SCIPgetLhsConstantSOC(scip, cons)
    ccall((:SCIPgetLhsConstantSOC, libscip), Cdouble, (Ptr{SCIP_}, Ptr{SCIP_CONS}), scip, cons)
end

function SCIPgetRhsVarSOC(scip, cons)
    ccall((:SCIPgetRhsVarSOC, libscip), Ptr{SCIP_VAR}, (Ptr{SCIP_}, Ptr{SCIP_CONS}), scip, cons)
end

function SCIPgetRhsCoefSOC(scip, cons)
    ccall((:SCIPgetRhsCoefSOC, libscip), Cdouble, (Ptr{SCIP_}, Ptr{SCIP_CONS}), scip, cons)
end

function SCIPgetRhsOffsetSOC(scip, cons)
    ccall((:SCIPgetRhsOffsetSOC, libscip), Cdouble, (Ptr{SCIP_}, Ptr{SCIP_CONS}), scip, cons)
end

function SCIPaddToNlpiProblemSOC(scip, cons, nlpi, nlpiprob, scipvar2nlpivar, names)
    ccall((:SCIPaddToNlpiProblemSOC, libscip), SCIP_RETCODE, (Ptr{SCIP_}, Ptr{SCIP_CONS}, Ptr{SCIP_NLPI}, Ptr{SCIP_NLPIPROBLEM}, Ptr{SCIP_HASHMAP}, UInt32), scip, cons, nlpi, nlpiprob, scipvar2nlpivar, names)
end


# Core traits and accessor functions

"""
`alpha(p)` extracts the alpha component of a color. For a color
without an alpha channel, it will always return 1.
"""
alpha(c::TransparentColor) = c.alpha
alpha(c::Color)   = oneunit(eltype(c))
alpha(c::RGB24)   = N0f8(1)
alpha(c::ARGB32)  = reinterpret(N0f8, (c.color >> 0x18) % UInt8)
alpha(c::AGray32) = reinterpret(N0f8, (c.color >> 0x18) % UInt8)
alpha(x::Number)  = convert(eltype(ccolor(Gray, typeof(x))), oneunit(x))  # ensures it's a type supported by Gray (which has widest eltype support)

"`red(c)` returns the red component of an `AbstractRGB` opaque or transparent color."
red(c::AbstractRGB   ) = c.r
red(c::TransparentRGB) = c.r
red(c::RGB24)  = reinterpret(N0f8, (c.color >> 0x10) % UInt8)
red(c::ARGB32) = reinterpret(N0f8, (c.color >> 0x10) % UInt8)

"`green(c)` returns the green component of an `AbstractRGB` opaque or transparent color."
green(c::AbstractRGB   ) = c.g
green(c::TransparentRGB) = c.g
green(c::RGB24)  = reinterpret(N0f8, (c.color >> 0x08) % UInt8)
green(c::ARGB32) = reinterpret(N0f8, (c.color >> 0x08) % UInt8)

"`blue(c)` returns the blue component of an `AbstractRGB` opaque or transparent color."
blue(c::AbstractRGB   ) = c.b
blue(c::TransparentRGB) = c.b
blue(c::RGB24)  = reinterpret(N0f8, c.color % UInt8)
blue(c::ARGB32) = reinterpret(N0f8, c.color % UInt8)

"`gray(c)` returns the gray component of a grayscale opaque or transparent color."
gray(c::Gray)    = c.val
gray(c::TransparentGray) = c.val
gray(c::Gray24)  = reinterpret(N0f8, c.color % UInt8)
gray(c::AGray32) = reinterpret(N0f8, c.color % UInt8)
gray(x::Number)  = convert(eltype(ccolor(Gray, typeof(x))), x)   # ensures it's a type supported by Gray

"""
`chroma(c)` returns the chroma of a `Lab`, `Luv` or their variants color.
!!! note
    The other color types (e.g. `RGB`, `HSV` or `YCbCr`) are not supported
    because their definitions of *chroma* are not clear. Colorfulness, chroma
    and saturation are defined as distinct aspects by the CIE.
"""
chroma(c::Union{C,AlphaColor{C},ColorAlpha{C}}) where {C<:Union{Lab,Oklab,DIN99,DIN99o,DIN99d}} = sqrt(c.a^2 + c.b^2)
chroma(c::Union{C,AlphaColor{C},ColorAlpha{C}}) where {C<:Luv} = sqrt(c.u^2 + c.v^2)
chroma(c::Union{C,AlphaColor{C},ColorAlpha{C}}) where {C<:Union{LCHab,LCHuv, LCHOklab}} = c.c

"""
`hue(c)` returns the hue in degrees. This function does not guarantee that the
return value is in [0, 360].
"""
hue(c::Union{C,AlphaColor{C},ColorAlpha{C}}) where {C<:Union{HSV,HSL,HSI,LCHab,LCHuv, LCHOklab}} = c.h
hue(c::Union{C,AlphaColor{C},ColorAlpha{C}}) where {C<:Union{Lab,Oklab,DIN99,DIN99o,DIN99d}} =
    (h = atand(c.b, c.a); h < 0 ? h + 360 : h)
hue(c::Union{C,AlphaColor{C},ColorAlpha{C}}) where {C<:Luv} =
    (h = atand(c.v, c.u); h < 0 ? h + 360 : h)

# fallbacks for compN
_comp(::Val{N}, c::Colorant) where N = getfield(c, N)
_comp(::Val{N}, c::AlphaColor) where N = getfield(c, N + 1)
_comp(::Val{N}, c::AlphaColorN{N}) where N = alpha(c)
_comp(::Val{N}, c::ColorAlphaN{N}) where N = alpha(c)

@noinline function _comp_error(c::ColorantN{N}, n::Int) where N
    io = IOBuffer()
    print(io, "attempt to access ", N, "-component color ")
    print(IOContext(io, :compact=>true), typeof(c), " with `comp", n, "`")
    throw(ArgumentError(String(take!(io))))
end

"""
`comp1(c)` extracts the first component you'd pass to the constructor
of the corresponding object.  For most color types without an alpha
channel, this is just the first field, but for types like `BGR` that
reverse the internal storage order this provides the value that you'd
use to reconstruct the color.

Specifically, for any `Color{T,3}`,

    c == typeof(c)(comp1(c), comp2(c), comp3(c))

returns true.
"""
comp1(c::Colorant) = _comp(Val(1), c)
comp1(c::Union{AbstractRGB, TransparentRGB}) = red(c)
comp1(c::Union{AbstractGray, TransparentGray}) = gray(c)

"`comp2(c)` extracts the second constructor argument (see `comp1`)."
comp2(c::Colorant) = _comp(Val(2), c)
comp2(c::ColorantN{1}) = _comp_error(c, 2)
comp2(c::Union{AbstractRGB, TransparentRGB}) = green(c)

"`comp3(c)` extracts the third constructor argument (see `comp1`)."
comp3(c::Colorant) = _comp(Val(3), c)
comp3(c::Union{ColorantN{1}, ColorantN{2}}) = _comp_error(c, 3)
comp3(c::Union{AbstractRGB, TransparentRGB}) = blue(c)

"`comp4(c)` extracts the fourth constructor argument (see `comp1`)."
comp4(c::Colorant) = _comp(Val(4), c)
comp4(c::Union{ColorantN{1}, ColorantN{2}, ColorantN{3}}) = _comp_error(c, 4)

"`comp5(c)` extracts the fifth constructor argument (see `comp1`)."
comp5(c::Colorant) = _comp(Val(5), c)
comp5(c::Union{ColorantN{1}, ColorantN{2}, ColorantN{3}, ColorantN{4}}) = _comp_error(c, 5)

"`color(c)` extracts the opaque color component from a Colorant (e.g., omits the alpha channel, if present)."
color(c::Color) = c
color(c::TransparentColorN{2,C}) where {C} = C(comp1(c))
color(c::TransparentColorN{3,C}) where {C} = C(comp1(c), comp2(c))
color(c::TransparentColorN{4,C}) where {C} = C(comp1(c), comp2(c), comp3(c))
color(c::TransparentColorN{5,C}) where {C} = C(comp1(c), comp2(c), comp3(c), comp4(c))

# Some of these traits exploit a nice trick: for subtypes, walk up the
# type hierarchy until we get to a stage where we can define the
# function in general

# recurse up the type hierarchy until you get to Colorant{T,N} for
# specific T,N.
to_top(::Type{Colorant{T,N}}) where {T,N} = Colorant{T,N}
@pure to_top(::Type{C}) where {C<:Colorant} = to_top(supertype(C))

to_top(c::Colorant) = to_top(typeof(c))

# Return the number of components in the color
# Note this is different from div(sizeof(c), sizeof(eltype(c))) (e.g., XRGB)
length(::Type{<:ColorantN{N}}) where N = N

length(c::Colorant) = length(typeof(c))

# eltype(RGB{Float32}) -> Float32
eltype(::Type{C}) where {C<:Colorant{T}} where {T} = T

eltype(c::Colorant) = eltype(typeof(c))

@pure function _parameter_upper_bound(t::UnionAll, idx)
    Base.rewrap_unionall((Base.unwrap_unionall(t)::DataType).parameters[idx], t)
end

# eltypes_supported(Colorant{T<:X}) -> X
function eltypes_supported(::Type{C}) where {C<:Colorant}
    Cb = base_colorant_type(C)
    isconcretetype(C) && C === Cb && return eltype(C)
    _eltypes_supported(Cb, supertype(Cb))
end
@pure _eltypes_supported(::Type{<:Colorant}, ::Type{C}) where {C<:Colorant} = _eltypes_supported(C, supertype(C))
_eltypes_supported(::Type{C}, ::Type) where {C<:Colorant} = _parameter_upper_bound(C, 1)

eltypes_supported(c::Colorant) = eltypes_supported(typeof(c))

"""
    issupported(C::Type, T::Type)::Bool

Returns `true` if `T` is a valid numeric eltype for `C<:Colorant`.
"""
issupported(::Type{C}, ::Type{T}) where {C<:Colorant,T} = T <: eltypes_supported(C)

"""
    CT = color_type(C::Type)
    CT = color_type(c::Colorant)

Return the type of the Color object, discarding any alpha channel.
For example,

    color_type(RGB)          === RGB
    color_type(RGB{Float32}) === RGB{Float32}
    color_type(ARGB{N0f8})   === RGB{N0f8}
    color_type(RGB(1,0,0))   === RGB{N0f8}

`color_type`, and related functions like [`base_color_type`](@ref),
[`base_colorant_type`](@ref), and [`ccolor`](@ref) are useful for manipulating types
when writing generic code.
"""
color_type(::Type{C}) where {C<:Color} = C
color_type(::Type{C}) where {C<:AlphaColor} = color_type(supertype(C)) # may help caching
color_type(::Type{C}) where {C<:ColorAlpha} = color_type(supertype(C)) # may help caching
function color_type(::Type{TC}) where {TC <: TransparentColor}
    _color_type(::Type{TC}) where {C, TC <: TransparentColor{C}} = C
    if TC isa UnionAll
        supertype(TC) <: TransparentColor && return color_type(supertype(TC))
        C1 = _parameter_upper_bound(TC, 1)
        color_type(C1) # simplify
    else
        _color_type(TC)
    end
end
color_type(::Type{Colorant{T,N}}) where {T,N} = Color{T,N}
color_type(::Type{ColorantN{N}}) where {N} = ColorN{N}
color_type(::Type{Colorant{T}}) where {T} = Color{T}
color_type(::Type{Colorant}) = Color
color_type(::Type{X}) where {X<:Number} = typeof(Gray(zero(X)))

color_type(c::Union{Colorant, Number}) = color_type(typeof(c))

"""
    Cbase = base_color_type(C::Type)
    Cbase = base_color_type(c::Colorant)

Return the Color type without alpha channel or a specified numeric element type.
`base_color_type` is similar to [`color_type`](@ref), except it "strips off" the
element type. It always returns a parametric type, which makes it convenient
for switching the numeric element type.

For example, compare

    base_color_type(RGB{N0f8})  === RGB
    base_color_type(RGBA{N0f8}) === RGB

vs

    color_type(RGB{N0f8})       === RGB{N0f8}

Switching element types can be done as follows:

    c64 = base_color_type(c){Float64}(color(c))

`base_color_type` discards any alpha channel. See [`base_colorant_type`](@ref)
if you want to preserve the alpha channel.
"""
base_color_type(::Type{C}) where {C<:Colorant} = base_colorant_type(color_type(C))
base_color_type(::Type{<:Number}) = Gray

base_color_type(x::Union{Colorant,Number}) = base_color_type(typeof(x))

## base_colorant_type implementation

base_colorant_type(::Type{C}) where {C<:Colorant} = isabstracttype(C) ? abstract_basetype(C) : basetype(C)
base_colorant_type(::Type{<:Number}) = Gray

@pure basetype(@nospecialize(C)) = Base.typename(C).wrapper

abstract_basetype(::Type{<:AbstractRGB}) = AbstractRGB
abstract_basetype(::Type{<:AbstractGray}) = AbstractGray
abstract_basetype(::Type{<:ColorN{N}}) where N = ColorN{N}
abstract_basetype(::Type{<:Color}) = Color
function abstract_basetype(::Type{TC}) where {TC<:TransparentColor}
    P = TC <: AlphaColor ? AlphaColor : TC <: ColorAlpha ? ColorAlpha : TransparentColor
    _abstract_transparent_basetype(TC, P, base_color_type(TC))
end
function _abstract_transparent_basetype(::Type{TC}, P, Cb) where {N, TC <: TransparentColorN{N}}
    Cb === Color && return P{C,T,N} where {T,C<:ColorN{N-1,T}}
    isabstracttype(Cb) || return P{Cb{T},T,N} where T
    return P{C,T,N} where {T,C<:Cb{T}}
end
function _abstract_transparent_basetype(::Type{TC}, P, Cb) where {TC <: TransparentColor}
    TC === P && return P
    isabstracttype(Cb) || return P{Cb{T},T} where T
    return P{C,T} where {T,C<:Cb{T}}
end
# This fallback dispatches to a separate function to control method ordering.
# Otherwise the generic ColorantN methods supersede the AlphaColor/ColorAlpha/Transparent methods
abstract_basetype(::Type{C}) where C = abstract_colorant_basetype(C)

abstract_colorant_basetype(::Type{<:ColorantN{N}}) where N = ColorantN{N}
abstract_colorant_basetype(::Type{<:Colorant})             = Colorant

"""
    Cbase = base_colorant_type(C::Type)
    Cbase = base_colorant_type(c::Colorant)

Return the Colorant type without specified numeric element type.

For example, compare

    base_colorant_type(ARGB{N0f8}) === ARGB

vs

    base_color_type(ARGB{N0f8})    === RGB
    color_type(ARGB{N0f8})         === RGB{N0f8}

When possible, `base_colorant_type` returns a parametric `UnionAll`, so
that `Cbase{T}` can be used to specify a colorant with element type `T`.
However, `base_colorant_type` is not guaranteed to return a parametric result,
for example

```jldoctest; setup = :(using ColorTypes)
julia> base_colorant_type(RGB24)
RGB24
```

In generic code, you can check whether `Cbase` is a `DataType` or `UnionAll`;
`Cbase{T}` will be valid only if it's a `UnionAll`.

# Usage example

A safe and easy way to switch the element type of a colorant value `c` to be `Float64` is

    Cbase = base_colorant_type(parametric_colorant(typeof(c)))
    c64 = Cbase{Float64}(c)

"""
base_colorant_type(c::Union{Colorant, Number}) = base_colorant_type(typeof(c))

"""
    Cp = parametric_colorant(C::Type)

Return a parametric colorant type.

# Examples

```jldoctest; setup = :(using ColorTypes, ColorTypes.FixedPointNumbers)
julia> parametric_colorant(RGB)
RGB

julia> parametric_colorant(RGB{Float32})
RGB{Float32}

julia> parametric_colorant(BGR)
BGR

julia> parametric_colorant(RGB24) == RGB{N0f8}
true
```
"""
parametric_colorant(::Type{C}) where C<:Colorant = C
parametric_colorant(::Type{RGB24}) = RGB{N0f8}
parametric_colorant(::Type{Gray24}) = Gray{N0f8}
parametric_colorant(::Type{ARGB32}) = ARGB{N0f8}
parametric_colorant(::Type{AGray32}) = AGray{N0f8}

"""
    CF = floattype(C::Type)

Promote storage data type of colorant type `C` to `AbstractFloat` while keeping the
`base_colorant_type` the same.

!!! info

    Non-parametric colorants will be promoted to corresponding parametric
    colorants. For example, `floattype(RGB24) == RGB{Float32}`.
"""
floattype(::Type{T}) where T <: Colorant =
    base_colorant_type(T){floattype(eltype(T))} # 1 parameter
# 0 parameter
floattype(::Type{RGB24})   = RGB{floattype(N0f8)}
floattype(::Type{Gray24})  = Gray{floattype(N0f8)}
floattype(::Type{ARGB32})  = ARGB{floattype(N0f8)}
floattype(::Type{AGray32}) = AGray{floattype(N0f8)}

@pure pureintersect(::Type{C1}, ::Type{C2}) where {C1,C2} = typeintersect(C1, C2)

"""
    Calpha, Cbase, T = colorsplit(C)

Split a color type `C` into three components: `T` is the numeric type,
`Cbase` is the `base_color_type(C)`, and `Calpha` is one of `Nothing`
(when `C` has no transparency), `AlphaColor`, or `ColorAlpha`.

If `C` is an abstract type, `Calpha` may be `TransparentColor` or `Any`.
"""
function colorsplit(::Type{C}) where C<:Colorant
    Calpha = C <: AlphaColor ? AlphaColor :
             C <: ColorAlpha ? ColorAlpha :
             C <: Color ? Nothing :
             C <: TransparentColor ? TransparentColor : Any
    Cbase = C <: Union{Color,TransparentColor} ? base_color_type(C) : Color
    return Calpha, Cbase, eltype(C)
end

"""
    C = ccolor(Cdest, Csrc)

`ccolor` ("concrete color") supports independent selection of colorspace and
numeric element type. `ccolor` chooses the numeric element type from
`Cdest` if available, but if not specified gets it from `Csrc`.

```jldoctest; setup = :(using ColorTypes, ColorTypes.FixedPointNumbers)
julia> ccolor(RGB, Gray{Float32})
RGB{Float32}

julia> ccolor(RGB, Gray{N0f8}) == RGB{N0f8}
true

julia> ccolor(RGB{Float32}, Gray{N0f8})
RGB{Float32}
```

Some colorspaces don't support `FixedPoint` numeric element types;
in such cases the `eltype_default` for `Cdest` is chosen:

```jldoctest; setup = :(using ColorTypes, ColorTypes.FixedPointNumbers)
julia> ccolor(HSV, RGB{N0f8})
HSV{Float32}
```

`Cdest` can be an abstract type, in which case `Csrc` must be in that
abstract color space.

```jldoctest; setup = :(using ColorTypes, ColorTypes.FixedPointNumbers)
julia> ccolor(Color{Float32}, RGB{N0f8})
RGB{Float32}

julia> ccolor(AbstractRGB{Float32}, BGR{N0f8})
BGR{Float32}

julia> ccolor(AbstractRGB{Float32}, HSV{Float32})
ERROR: in ccolor, empty intersection between AbstractRGB and HSV
[...]
```
`ccolor` will throw an error when no unambiguous concrete type can be determined.
In the previous case, `AbstractRGB{Float32}` encompases `RGB{Float32}` and `BGR{Float32}`.

`ccolor` is most useful in defining other methods.
For example, to allow users to write `convert(RGB, c)` without having to specify the
numeric element type of `RGB`, define

```
convert(::Type{C}, p::Colorant) where C<:Colorant = cnvt(ccolor(C,typeof(p)), p)
```

where `cnvt` is the function that performs explicit conversion.
"""
function ccolor(::Type{Cdest}, ::Type{Csrc}) where {Cdest<:Colorant, Csrc<:Union{Number,Colorant}}
    Cdestalpha, Cdestbase, Tdest = colorsplit(Cdest)
    if Csrc <: Number
        Cdestbase <: Union{AbstractGray, AbstractRGB} || throw(ColorTypeResolutionError(:ccolor, "no automatic conversion from", Csrc, Cdestbase))
        Csrcalpha, Csrcbase, Tsrc = Any, Color, Csrc
    else
        Csrcalpha, Csrcbase, Tsrc = colorsplit(Csrc)
    end
    # Step 1: pick the base color type
    if isabstracttype(Cdestbase)
        C = pureintersect(Cdestbase, Csrcbase)
        C === Union{} && throw(ColorTypeResolutionError(:ccolor, "empty intersection between", Cdestbase, Csrcbase))
        isabstracttype(C) && throw(ColorTypeResolutionError(:ccolor, "abstract intersection between", Cdestbase, Csrcbase))
    else
        C = Cdestbase
    end
    # Step 2: combine with the alpha representation
    if Cdestalpha === Nothing
    else
        Calpha = (Cdestalpha === TransparentColor || Cdestalpha === Any) ? Csrcalpha : Cdestalpha
        Cdestalpha === TransparentColor && Calpha === Nothing && throw(ColorTypeResolutionError(:ccolor, "ambiguous alpha storage between", Cdest, Csrc))
        if Calpha !== Nothing
            Calpha === TransparentColor && throw(ColorTypeResolutionError(:ccolor, "ambiguous alpha storage between", Cdest, Csrc))
            C = Calpha <: AlphaColor ? alphacolor(C) :
                Calpha <: ColorAlpha ? coloralpha(C) : error("unexpected Calpha ", Calpha)
        end
    end
    # Step 3: assign the eltype
    if !isa(C, UnionAll)
        eltype(C) <: Tdest && return C   # RGB24 etc.
        error("nonparametric type ", C, " has ambiguous destination ", Cdest)
    end
    isabstracttype(Tdest) || !issupported(C, Tdest) || return C{Tdest}
    T = pureintersect(Tdest, Tsrc)
    T === Union{} && throw(ColorTypeResolutionError(:ccolor, "empty intersection between", Tdest, Tsrc))
    T === Any && return C
    issupported(C, T) && return C{T}
    Tdef = eltype_default(C)
    T<:Integer && return C{Tdef}
    Tf = floattype(Tsrc)
    issupported(C, Tf) && return C{Tf}
    return C{Tdef}
end

isfinite(c::Colorant) = mapreducec(isfinite, &, true, c)
isinf(c::Colorant) = mapreducec(isinf, |, false, c)
isnan(c::Colorant) = mapreducec(isnan, |, false, c)

"""
    ColorTypes.nan(C)

Return a color instance of the specified type in which all components are NaN.

# Examples
```jldoctest; setup = :(using ColorTypes)
julia> ColorTypes.nan(RGB{Float32})
RGB{Float32}(NaN, NaN, NaN)

julia> ColorTypes.nan(AHSV{Float64})
AHSV{Float64}(NaN, NaN, NaN, NaN)
```
"""
nan(::Type{T}) where {T<:AbstractFloat} = convert(T, NaN)
nan(::Type{C}) where {T<:AbstractFloat, N, C<:Colorant{T, N}} = C(ntuple(_ -> nan(T), Val(N))...)

zero(::Type{C}) where {N, C<:ColorantN{N}} = C(ntuple(_ -> 0, Val(N))...)
zero(c::Colorant) = zero(typeof(c))

oneunit(::Type{C}) where {C<:Colorant} = throw_oneunit_error(C)
@noinline function throw_oneunit_error(@nospecialize(C))
    throw(ArgumentError("`oneunit` for $C is not defined. Perhaps the meaning is not clear."))
end
oneunit(::Type{C}) where {C<:AbstractGray} = C(1)
# It's not clear what `oneunit` means for most Color3s,
# but for AbstractRGB, XYZ, and LMS, it's OK
oneunit(::Type{C}) where {C<:AbstractRGB}      = C(1, 1, 1)
oneunit(::Type{C}) where {C<:Union{XYZ, LMS}}  = C(1, 1, 1)
oneunit(::Type{C}) where {C<:TransparentColor} = C(oneunit(color_type(C)))
oneunit(::Type{C}) where {C<:Union{AGray, GrayA}} = C(1, 1) # workaround for inconsistent `color_type`
oneunit(c::Colorant) = oneunit(typeof(c))

one(::Type{C}) where {C<:Colorant} = one(eltype_default(C))
one(::Type{C}) where {T, C<:Colorant{T}} = one(T)
one(c::Colorant) = one(typeof(c))

Base.broadcastable(x::Colorant) = Ref(x)


module OBJMeshes

export read_obj, write_obj, tri_vertex_str
export cube_mesh, cylinder_mesh, sphere_mesh, estimate_normals, portal_mesh, portal_background_mesh
export OBJTriangle, OBJMesh

using FileIO
using LinearAlgebra

#push!(LOAD_PATH, pwd())
using ..GfxBase

""" OBJTriangle
A struct that represents a single triangle in a mesh. """
mutable struct OBJTriangle
    positions::Array{Int, 1} # vertex position indices
    uvs::Array{Int, 1} # vertex texture coordinate indices
    normals::Array{Int, 1} # normal vector indices
end

""" OBJMesh
A struct that represents an indexed triangle mesh for reading from
or writing to OBJ format. """
mutable struct OBJMesh
    positions::Array{Vec3, 1} # all vertex positions
    uvs::Array{Vec2, 1} # all texture coordinates
    normals::Array{Vec3, 1} # all vertex normals
    triangles::Array{OBJTriangle, 1} # the OBJTriangles belonging to the mesh
end

""" read_obj(obj_filename)
Read a mesh in OBJ format from file obj_filename."""
function read_obj(obj_filename)
    m = OBJMesh([], [], [], []) # create a mesh
    open(obj_filename) do f
        for (line_number, line) in enumerate(eachline(f))
            if line == "" || line[1] == "#"
                continue # skip comments
            end
            # Read the line and add its contents to the correct field of m:
            tokens = split(strip(line))
            if tokens[1] == "v" # vertex
                push!(m.positions, Vec3([parse(Float64, x) for x in tokens[2:end]]...))
            elseif tokens[1] == "vt" # vertex texture
                push!(m.uvs, Vec2([parse(Float64, x) for x in tokens[2:end]]...))
            elseif tokens[1] == "vn" # vertex normal
                push!(m.normals, Vec3([parse(Float64, x) for x in tokens[2:end]]...))
            elseif tokens[1] == "f"
                # create a OBJTriangle face:
                points = []
                uvs = []
                normals = []
                # handle faces with no texture and/or normals
                for corner in tokens[2:end]
                    indices = split(corner, '/')
                    if length(indices) == 3 # all 3 present, third is a normal
                        push!(normals, parse(Int, indices[3]))
                    end
                    if length(indices) >= 2 && indices[2] != ""
                        # if there are 2 or more and the second isn't blank, it's a texture
                        push!(uvs, parse(Int, indices[2]))
                    end
                    if length(indices) >= 1 # first value is the position
                        push!(points, parse(Int, indices[1]))
                    else # unless it has none, in which case it's not valid
                        error("in line $line_number: face vertex $corner could not be parsed")
                    end
                end
                # create the triangle and add it to the triangles array
                push!(m.triangles, OBJTriangle(points, uvs, normals))
            end
        end
    end
    return m
end

""" write_obj(obj_filename)
Write the given mesh in OBJ format to file obj_filename."""
function write_obj(obj_filename, mesh::OBJMesh)
    open(obj_filename, "w") do f
        # write all positions:
        for v in mesh.positions
            write(f, "v $(v[1]) $(v[2]) $(v[3])\n")
        end

        # write all texture coords:
        for v in mesh.uvs
            write(f, "vt $(v[1]) $(v[2])\n")
        end
        # write all normals:
        for v in mesh.normals
            write(f, "vn $(v[1]) $(v[2]) $(v[3])\n")
        end

        # write all triangles:
        for tri in mesh.triangles
            write(f, "f $(tri_vertex_str(tri))\n")
        end

    end

end

""" tri_vertex_str(triangle)
Return a string with the indices of applicable positions, texture coordinates,
and normals for a given triangle according to the OBJ specification.
In particular, if p, u, and n are position, vertex and normal, each corner
of the triangle is represented as one of the following:
    p       (position only)
    p/u     (position and texture)
    p//n    (position and normal)
    p/u/n   (position, texture, and normal)
"""
function tri_vertex_str(triangle::OBJTriangle)
    # determine whether textures and normals are present:
    write_uv = length(triangle.uvs) == length(triangle.positions)
    write_normals = length(triangle.normals) == length(triangle.positions)
    corners = []
    for i = 1:3
        output = "$(triangle.positions[i])"
        if write_uv && !write_normals
            output = output * "/$(triangle.uvs[i])" # * does concatenation(!)
        elseif !write_uv && write_normals
            output = output * "//$(triangle.normals[i])"
        elseif write_uv && write_normals
            output = output * "/$(triangle.uvs[i])/$(triangle.normals[i])"
        end
        push!(corners, output)
    end
    join(corners, " ")
end

""" cube_mesh()
Return a new OBJMesh representing a 2x2x2 cube centered at the origin and
axis-aligned. """
function cube_mesh()
    positions = []
    uvs = []
    normals = []
    triangles = []
    # key to comments:
    # L/R = x = right/left
    # B/T = y = top/bottom
    # C/F = z = close/far
    push!(positions, Vec3( 1, -1, -1)) # 1 RBC
    push!(positions, Vec3( 1, -1,  1)) # 2 RBF
    push!(positions, Vec3(-1, -1,  1)) # 3 LBF
    push!(positions, Vec3(-1, -1, -1)) # 4 LBC
    push!(positions, Vec3( 1,  1, -1)) # 5 RTC
    push!(positions, Vec3( 1,  1,  1)) # 6 RTF
    push!(positions, Vec3(-1,  1,  1)) # 7 LTF
    push!(positions, Vec3(-1,  1, -1)) # 8 LTC

    # texture coordinates:
    push!(uvs, Vec2(1, 1)) # TR
    push!(uvs, Vec2(0, 1)) # TL
    push!(uvs, Vec2(0, 0)) # BL
    push!(uvs, Vec2(1, 0)) # BR

    #by removing pre-made normals from our objects, we're able to rotate them and calculate new normals (gets rid of black surfaces!)

    # normals:
    #push!(normals, Vec3( 1, 0, 0)) # R
    #push!(normals, Vec3(-1, 0, 0)) # L
    #push!(normals, Vec3( 0, 1, 0)) # U
    #push!(normals, Vec3( 0,-1, 0)) # D
    #push!(normals, Vec3( 0, 0, 1)) # C
    #push!(normals, Vec3( 0, 0,-1)) # F

    # 8 faces, 2 triangles each
    push!(triangles, OBJTriangle([1,2,3], [1,2,3], [])) # bottom face 1
    push!(triangles, OBJTriangle([1,3,4], [1,3,4], [])) # bottom face 2
    push!(triangles, OBJTriangle([1,5,6], [4,1,2], [])) # right face 1
    push!(triangles, OBJTriangle([1,6,2], [4,2,3], [])) # right face 2
    push!(triangles, OBJTriangle([2,6,7], [4,1,2], [])) # far face 1
    push!(triangles, OBJTriangle([2,7,3], [4,2,3], [])) # far face 2
    push!(triangles, OBJTriangle([3,7,8], [2,3,4], [])) # left face 1
    push!(triangles, OBJTriangle([3,8,4], [2,4,1], [])) # left face 2
    push!(triangles, OBJTriangle([4,8,5], [2,3,4], [])) # far face 1
    push!(triangles, OBJTriangle([4,5,1], [2,4,1], [])) # far face 2
    push!(triangles, OBJTriangle([5,8,7], [1,2,3], [])) # top face 1
    push!(triangles, OBJTriangle([5,7,6], [1,3,4], [])) # top face 2

    # julia automatically returns the last value in the function:
    OBJMesh(positions, uvs, normals, triangles)

end

function portal_mesh(x, y)

    positions = []
    uvs = []
    normals = []
    triangles = []
    push!(positions, Vec3(0,0,1)) # top of cap
    #push!(normals, Vec3(0,0,1))
    n = 20
    for i = 1:n+1
        ratio = i/n
        theta = 2*pi*ratio
        push!(positions, Vec3(-sin(theta)*x, -cos(theta)*y, 1))
        #push!(normals, Vec3(-sin(theta),  -cos(theta), 1))
    end
	length_pos = length(positions)
     for i=1:length_pos-1
       push!(triangles, OBJTriangle([i, 1,i+1],[i+length_pos,1,i+length_pos],[]))
     end
     OBJMesh(positions, uvs, normals, triangles)

end

#a copy of the portal mesh function that (hopefully) will be recognized as not-a-portal
function portal_background_mesh(x, y)
    positions = []
    uvs = []
    normals = []
    triangles = []
    push!(positions, Vec3(0,0,1)) # top of cap
    n = 20
    for i = 1:n+1
        ratio = i/n
        theta = 2*pi*ratio
        push!(positions, Vec3(-sin(theta)*x, -cos(theta)*y, 1))
    end
	length_pos = length(positions)
     for i=1:length_pos-1
       push!(triangles, OBJTriangle([i, 1,i+1],[i+length_pos,1,i+length_pos],[]))
     end
     OBJMesh(positions, uvs, normals, triangles)
end

""" cylinder_mesh(n)
Return a new OBJMesh object approximation of a cylinder with radius 1 and
height 2, centered at the origin. The logitudinal axis is aligned with y, and
it is tesselated with n divisions arranged radially around the outer surface.
The ends of the cylinder are disc-shaped caps parallel to the xz plane.
"""
function cylinder_mesh(divisionsU)
  positions = []
  uvs = []
  normals = []
  triangles = []

  n = divisionsU # change to n for ease of typing
  push!(positions, Vec3(0,1,0)) # top of cap
  push!(normals, Vec3(0,1,0))

  push!(positions, Vec3(0,-1,0)) # bottom of cap
  push!(normals, Vec3(0,-1,0))

  push!(positions, Vec3(0,1,-1)) # start of shell
  push!(positions, Vec3(0,-1,-1))
  push!(normals, Vec3(0,0,-1))

  push!(uvs, Vec2(.75, .75)) # initial uv coordinates
  push!(uvs, Vec2(.25, .75))
  push!(uvs, Vec2(0, .5))
  push!(uvs, Vec2(0, 0))
  # positions
  for i = 1:n-1
    ratio = i/n
    theta = 2*pi*ratio
    push!(positions, Vec3(-sin(theta), 1, -cos(theta)))
    push!(positions, Vec3(-sin(theta), -1, -cos(theta)))
    push!(normals, Vec3(-sin(theta), 0, -cos(theta)))
    push!(uvs, Vec2(ratio, .5)) # uvs for shell
    push!(uvs, Vec2(ratio, 0))
  end

  length_pos = length(positions) # for later use
  length_norm = length(normals)

  push!(uvs, Vec2(1, .5)) # missing coordinates
  push!(uvs, Vec2(1, 0))
  push!(uvs, Vec2(.75, 1))
  push!(uvs, Vec2(.25, .5))

  # uvs for caps
  for i=5:length_pos
      x = positions[i][1] # -sin(θ)
      y = positions[i][2] # y ∈ (-1,1)
      z = positions[i][3] # -cos(θ)
    if y == 1.0 # top cap
      push!(uvs, Vec2((x+1)/4+.5, (1-z)/4+.5))
    else # bottom cap
      push!(uvs, Vec2((x+1)/4, (z+1)/4+.5))
    end
  end

  n1 = 3 # normal counters
  n2 = 3

  # create the shell ignoring the top and bottom coordinates
  for i =3:2:length_pos-3
    # create triangles in a ccw order, texture coords are same index
    push!(triangles, OBJTriangle([i,i+1, i+2],[i,i+1,i+2],
                                 [n1,n1,n1+1]))
    push!(triangles, OBJTriangle([i+1,i+3, i+2],[i+1,i+3,i+2]
                                 ,[n2, n2+1, n2+1]))
    n1 += 1 # normals increment half as fast as the shell
    n2 += 1
  end

  # last bit of the shell
  push!(triangles, OBJTriangle([length_pos-1,length_pos,3],[length_pos-1,length_pos,length_pos+1],
                               [length_norm, length_norm, 3]))
  push!(triangles, OBJTriangle([length_pos,4,3],[length_pos, length_pos+2, length_pos+1],
                               [length_norm,3 ,3 ]))

  # create the caps for the cylinder
  for i=3:2:length_pos-2
    push!(triangles, OBJTriangle([i, i+2, 1],[i+length_pos,i+length_pos+2,1],[1,1,1]))
    push!(triangles, OBJTriangle([i+1, 2, i+3],[i+length_pos+1,2,i+length_pos+3],[2,2,2]))
  end

  # last bit of the caps
  push!(triangles, OBJTriangle([length_pos-1, 3, 1], [2*length_pos-1,length_pos+3,1], [1,1,1]))
  push!(triangles, OBJTriangle([length_pos, 2, 4], [2*length_pos,2,length_pos+4], [2,2,2]))

  OBJMesh(positions,uvs ,normals,triangles)
end

""" sphere_mesh(n, m
Create a Latitude-Longitude-tesselated approximation of a sphere with radius 1
centered at the origin. There are n divisions around the equator and m
divisions from pole to pole along each line of longitude. The North pole is at
(0,1,0), the South pole at (0,-1,0)((-1, and points on the Greenwich meridian are
in the x = 0 plane with z > 0. The u texture coordinate depends on longitude,
with u=0 at 180 degrees West and u=1 at 180 degrees East. The v texture
coordinate varies with latitude with v=0 at the South pole and v=1 at the North
pole. Normals should be normal to the ideal sphere surface. See the assignment
for a diagram and further details. """
function sphere_mesh(n, m)
    positions = []
    uvs = []
    normals = []
    triangles = []

    u = 2 * pi / n # normalize
    v = pi / m # normalize

    push!(positions, Vec3(0, -1, 0)) # bottom pole
    push!(normals, Vec3(0,-1,0))

    push!(positions, Vec3(0, 1, 0)) # top pole
    push!(normals, Vec3(0,1,0))

    # In this next block we convert x,y,z to spherical coordinates and
    # add the vectors to the positions array
    for i=1:m-1
      theta = (pi / 2) - (i * v) # π/2 to -π/2
      y = sin(theta) # Spherical y coord
      for j=0:n-1
        phi = j * u
        x = cos(theta) * sin(phi) # Spherical x coord
        z = cos(theta) * cos(phi) # spherical y coord
        push!(positions, Vec3(-x, -y, -z)) # negative as we start from the bottom
        push!(normals, Vec3(-x,-y,-z))
      end
    end
    length_pos = length(positions)
    # uv coords
    for i=0:m
      for j=0:n
        push!(uvs, Vec2(j/n,i/m))
      end
    end

    # create the bottom "ring"
    for i=1:n-1
      push!(triangles, OBJTriangle([1, i+3,i+2],[i,i+n+1,i+n],[1,i+3,i+2]))
    end

    # missing piece of bottom ring
    push!(triangles, OBJTriangle([1,3,n+2],[n, 2*n+1,2*n],[1,3,n+2]))

    t1, t2 = n+1,2*n+2 # texture counters
    for i=3:length_pos-n
      # if i is a multiple of m we subtract m from the first and second coordinates, respectively.
      if ((i-2)%n) == 0
       push!(triangles,OBJTriangle([i,i-n+1,i+n],[t1,t1+1,t2],[i,i-n+1,i+n]))
       push!(triangles,OBJTriangle([i-n+1,i+1,i+n],[t1+1,t2+1,t2],[i-n+1,i+1,i+n]))
       t1 +=2 # textures increment twice as fast
       t2 += 2
       continue
      end

     # for most of the triangles, i is not a multiple of m, add m to 3rd coord.
     push!(triangles, OBJTriangle([i, i+1,i+n],[t1,t1+1,t2],[i, i+1,i+n]))
     push!(triangles, OBJTriangle([i+1,i+1+n,i+n],[t1+1,t2+1,t2],[i+1,i+1+n,i+n]))
     t1 += 1 # textures increment normally here
     t2 += 1
    end

    # top "ring"
    for i =length_pos-n+1:length_pos-1
      push!(triangles,OBJTriangle([i,i+1,2],[t1,t1+1,t1+1+n],[i,i+1,2]))
      t1 += 1
      t2 += 1
    end
    # missing bit of the top "ring"
    push!(triangles,OBJTriangle([length_pos,length_pos-n+1,2],[t1,t1+1,t1+1+n],
                                [length_pos, length_pos-n+1,2]))

   deleteat!(uvs, 1) # delete the first/last index of texture coords as it is unneeded
   deleteat!(uvs, length(uvs))
   OBJMesh(positions,uvs, normals, triangles)
end

"""
    estimate_normals(mesh::OBJMesh)
Estimates normals for the given mesh. Overwrites any existing normals and returns a new OBJMesh object.
"""
function estimate_normals(mesh::OBJMesh)
  normals = []
  # cerate a normal for every position
  for  x in mesh.positions
    push!(normals, Vec3(0,0,0))
  end
  # average all the normals for every triangle
  for x in mesh.triangles
    a = mesh.positions[x.positions[1]]
    b = mesh.positions[x.positions[2]]
    c = mesh.positions[x.positions[3]]
    # definition of computing a normal
    cr = cross(b-a, c-a)
    normals[x.positions[1]] += cr/norm(cr)
    normals[x.positions[2]] += cr/norm(cr)
    normals[x.positions[3]] += cr/norm(cr)
  end

  tmp = [] # create another array to normalize the normals
  for i in normals
    i = i/norm(i) # normalize
    push!(tmp, i)
  end

  normals = tmp # for the return value
  for x in mesh.triangles
    x.normals = x.positions # normals have same index as positions per the reference OBJs
  end
  OBJMesh(mesh.positions, mesh.uvs, normals, mesh.triangles)
end

end # module OBJMeshes


module Sources

using Adapt
using DocStringExtensions

export AbstractSourceLW,
    SourceLWNoScat,
    SourceLW2Str,
    SourceSW2Str,
    source_func_longwave,
    source_func_shortwave

"""
    AbstractSourceLW{FT,FTA1D,FTA2D}

Abstract longwave source for no-scattering and two stream longwave solvers.
"""
abstract type AbstractSourceLW{FT,FTA1D,FTA2D} end

"""
    SourceLWNoScat{FT,FTA1D,FTA2D} <: AbstractSourceLW{FT,FTA1D,FTA2D}

Longwave sources: computed at layer center, layer edges,
and at the surface for no scattering calculations

# Fields

$(DocStringExtensions.FIELDS)
"""
struct SourceLWNoScat{
    FT<:AbstractFloat,
    FTA1D<:AbstractArray{FT,1},
    FTA2D<:AbstractArray{FT,2},
} <: AbstractSourceLW{FT,FTA1D,FTA2D}
    "Planck source at layer average temperature `[W/m2]` `(nlay, ncol)`"
    lay_source::FTA2D
    "Planck source at layer edge in increasing ilay direction `[W/m2]` `(nlay, ncol)`, includes spectral weighting that accounts for state-dependent frequency to g-space mapping"
    lev_source_inc::FTA2D
    "Planck source at layer edge in decreasing ilay direction `[W/m2]` `(nlay, ncol)`, includes spectral weighting that accounts for state-dependent frequency to g-space mapping"
    lev_source_dec::FTA2D
    "Surface source `[W/m2]` `(ncol)`"
    sfc_source::FTA1D
    "temporary storage array used in no scattering GPU calculations `(nlay, ncol)`"
    src_up::FTA2D
    "temporary storage array used in no scattering GPU calculations `(nlay, ncol)`"
    src_dn::FTA2D
end
Adapt.@adapt_structure SourceLWNoScat

function SourceLWNoScat(
    ::Type{FT},
    ::Type{DA},
    nlay::I,
    ncol::I,
) where {FT<:AbstractFloat,I<:Int,DA}
    FTA1D = DA{FT,1}
    FTA2D = DA{FT,2}

    lay_source = FTA2D(undef, nlay, ncol) # lay_source
    lev_source_inc = FTA2D(undef, nlay, ncol) # lev_source_inc
    lev_source_dec = FTA2D(undef, nlay, ncol) # lev_source_dec
    sfc_source = FTA1D(undef, ncol)       # sfc_source
    src_up = FTA2D(undef, nlay, ncol) # src_up
    src_dn = FTA2D(undef, nlay, ncol) # src_dn

    return SourceLWNoScat{
        eltype(lay_source),
        typeof(sfc_source),
        typeof(lay_source),
    }(
        lay_source,
        lev_source_inc,
        lev_source_dec,
        sfc_source,
        src_up,
        src_dn,
    )
end

"""
    SourceLW2Str{FT,FTA1D,FTA2D} <: AbstractSourceLW{FT,FTA1D,FTA2D}

Longwave sources: computed at layer center, layer edges,
and at the surface for 2-stream calculations

# Fields

$(DocStringExtensions.FIELDS)
"""
struct SourceLW2Str{
    FT<:AbstractFloat,
    FTA1D<:AbstractArray{FT,1},
    FTA2D<:AbstractArray{FT,2},
} <: AbstractSourceLW{FT,FTA1D,FTA2D}
    "Planck source at layer average temperature `[W/m2]` `(nlay, ncol)`"
    lay_source::FTA2D
    "Planck source at layer edge in increasing ilay direction `[W/m2]` `(nlay, ncol)`, includes spectral weighting that accounts for state-dependent frequency to g-space mapping"
    lev_source_inc::FTA2D
    "Planck source at layer edge in decreasing ilay direction `[W/m2]` `(nlay, ncol)`, includes spectral weighting that accounts for state-dependent frequency to g-space mapping"
    lev_source_dec::FTA2D
    "Surface source `[W/m2]` `(ncol)`"
    sfc_source::FTA1D
    "level source `[W/m2]` `(nlay+1, ncol)`, used in 2 stream calculations"
    lev_source::FTA2D
    "temporary storage array, used in 2 stream calculations `(nlay, ncol)`"
    src_up::FTA2D
    "temporary storage array, used in 2 stream calculations `(nlay, ncol)`"
    src_dn::FTA2D
    "temporary storage array, used in 2 stream calculations `(nlay, ncol)`"
    Rdif::FTA2D
    "temporary storage array, used in 2 stream calculations `(nlay, ncol)`"
    Tdif::FTA2D
    "temporary storage array, used in 2 stream calculations `(nlay + 1, ncol)`"
    albedo::FTA2D
    "temporary storage array, used in 2 stream calculations `(nlay + 1, ncol)`"
    src::FTA2D
end
Adapt.@adapt_structure SourceLW2Str

function SourceLW2Str(
    ::Type{FT},
    ::Type{DA},
    nlay::I,
    ncol::I,
) where {FT<:AbstractFloat,I<:Int,DA}
    FTA1D = DA{FT,1}
    FTA2D = DA{FT,2}

    lay_source = FTA2D(undef, nlay, ncol) # lay_source
    lev_source_inc = FTA2D(undef, nlay, ncol) # lev_source_inc
    lev_source_dec = FTA2D(undef, nlay, ncol) # lev_source_dec
    sfc_source = FTA1D(undef, ncol) # sfc_source
    lev_source = FTA2D(undef, nlay + 1, ncol) # lev_source
    src_up = FTA2D(undef, nlay, ncol) # src_up
    src_dn = FTA2D(undef, nlay, ncol) # src_dn
    Rdif = FTA2D(undef, nlay, ncol) # Rdif
    Tdif = FTA2D(undef, nlay, ncol) # Tdif
    albedo = FTA2D(undef, nlay + 1, ncol) # albedo
    src = FTA2D(undef, nlay + 1, ncol) # src

    return SourceLW2Str{
        eltype(lay_source),
        typeof(sfc_source),
        typeof(lay_source),
    }(
        lay_source,
        lev_source_inc,
        lev_source_dec,
        sfc_source,
        lev_source,
        src_up,
        src_dn,
        Rdif,
        Tdif,
        albedo,
        src,
    )
end

"""
    source_func_longwave(
        ::Type{FT},
        ncol::I,
        nlay::I,
        OPC::Symbol,
        ::Type{DA},
    ) where {FT<:AbstractFloat,I<:Int,DA}

Initializes the longwave source for one scalar and two stream simulations.
"""
function source_func_longwave(
    ::Type{FT},
    ncol::I,
    nlay::I,
    OPC::Symbol,
    ::Type{DA},
) where {FT<:AbstractFloat,I<:Int,DA}
    lay_source = DA{FT,2}(undef, nlay, ncol)
    lev_source_inc = DA{FT,2}(undef, nlay, ncol)
    lev_source_dec = DA{FT,2}(undef, nlay, ncol)
    sfc_source = DA{FT,1}(undef, ncol)
    src_up = DA{FT,2}(undef, nlay, ncol)
    src_dn = DA{FT,2}(undef, nlay, ncol)

    if OPC === :OneScalar
        return SourceLWNoScat{
            eltype(lay_source),
            typeof(sfc_source),
            typeof(src_up),
        }(
            lay_source,
            lev_source_inc,
            lev_source_dec,
            sfc_source,
            src_up,
            src_dn,
        )
    else
        lev_source = DA{FT,2}(undef, nlay + 1, ncol)
        Rdif = DA{FT,2}(undef, nlay, ncol)
        Tdif = DA{FT,2}(undef, nlay, ncol)
        albedo = DA{FT,2}(undef, nlay + 1, ncol)
        src = DA{FT,2}(undef, nlay + 1, ncol)
        return SourceLW2Str{FT,typeof(sfc_source),typeof(src_up)}(
            lay_source,
            lev_source_inc,
            lev_source_dec,
            sfc_source,
            lev_source,
            src_up,
            src_dn,
            Rdif,
            Tdif,
            albedo,
            src,
        )
    end
end

"""
    SourceSW2Str{FT,FTA1D,FTA2D}

Shortwave sources: computed at layer center, layer edges,
and at the surface for 2-stream calculations

# Fields

$(DocStringExtensions.FIELDS)
"""
struct SourceSW2Str{
    FT<:AbstractFloat,
    FTA1D<:AbstractArray{FT,1},
    FTA2D<:AbstractArray{FT,2},
}
    "surface source `(ncol)`"
    sfc_source::FTA1D
    "albedo `(nlay + 1, ncol)`"
    albedo::FTA2D
    "temporary storage array, used in 2 stream calculations `(nlay, ncol)`"
    src_up::FTA2D
    "temporary storage array, used in 2 stream calculations `(nlay, ncol)`"
    src_dn::FTA2D
    "temporary storage array, used in 2 stream calculations `(nlay, ncol)`"
    Rdif::FTA2D
    "temporary storage array, used in 2 stream calculations `(nlay, ncol)`"
    Tdif::FTA2D
    "temporary storage array, used in 2 stream calculations `(nlay, ncol)`"
    Rdir::FTA2D
    "temporary storage array, used in 2 stream calculations `(nlay, ncol)`"
    Tdir::FTA2D
    "temporary storage array, used in 2 stream calculations `(nlay, ncol)`"
    Tnoscat::FTA2D
    "temporary storage array, used in 2 stream calculations `(nlay + 1, ncol)`"
    src::FTA2D
end
Adapt.@adapt_structure SourceSW2Str

function SourceSW2Str(
    ::Type{FT},
    ::Type{DA},
    nlay::I,
    ncol::I,
) where {FT<:AbstractFloat,I<:Int,DA}
    FTA1D = DA{FT,1}
    FTA2D = DA{FT,2}

    sfc_source = FTA1D(undef, ncol)
    albedo = FTA2D(undef, nlay + 1, ncol)
    src_up = FTA2D(undef, nlay, ncol)
    src_dn = FTA2D(undef, nlay, ncol)
    Rdif = FTA2D(undef, nlay, ncol)
    Tdif = FTA2D(undef, nlay, ncol)
    Rdir = FTA2D(undef, nlay, ncol)
    Tdir = FTA2D(undef, nlay, ncol)
    Tnoscat = FTA2D(undef, nlay, ncol)
    src = FTA2D(undef, nlay + 1, ncol)

    return SourceSW2Str{eltype(sfc_source),typeof(sfc_source),typeof(albedo)}(
        sfc_source,
        albedo,
        src_up,
        src_dn,
        Rdif,
        Tdif,
        Rdir,
        Tdir,
        Tnoscat,
        src,
    )
end

"""
    source_func_shortwave(
        ::Type{FT},
        ncol::I,
        nlay::I,
        opc::Symbol,
        ::Type{DA},
    ) where {FT<:AbstractFloat,I<:Int,DA}

Initializes the shortwave source for one scalar and two stream simulations.
"""
function source_func_shortwave(
    ::Type{FT},
    ncol::I,
    nlay::I,
    opc::Symbol,
    ::Type{DA},
) where {FT<:AbstractFloat,I<:Int,DA}
    if opc == :OneScalar
        return nothing
    else
        sfc_source = DA{FT}(undef, ncol)
        albedo = DA{FT}(undef, nlay + 1, ncol)
        src_up = DA{FT}(undef, nlay, ncol)
        src_dn = DA{FT}(undef, nlay, ncol)
        Rdif = DA{FT}(undef, nlay, ncol)
        Tdif = DA{FT}(undef, nlay, ncol)
        Rdir = DA{FT}(undef, nlay, ncol)
        Tdir = DA{FT}(undef, nlay, ncol)
        Tnoscat = DA{FT}(undef, nlay, ncol)
        src = DA{FT}(undef, nlay + 1, ncol)
        return SourceSW2Str{FT,typeof(sfc_source),typeof(src_up)}(
            sfc_source,
            albedo,
            src_up,
            src_dn,
            Rdif,
            Tdif,
            Rdir,
            Tdir,
            Tnoscat,
            src,
        )
    end
end

end


const SVF = MOI.SingleVariable
const VVF = MOI.VectorOfVariables
const SAF{T} = MOI.ScalarAffineFunction{T}
const VAF{T} = MOI.VectorAffineFunction{T}

const VI = MOI.VariableIndex
const CI = MOI.ConstraintIndex

const GT = MOI.GreaterThan
const LT = MOI.LessThan
const ET = MOI.EqualTo

const SCALAR_SETS = Union{
    MOI.GreaterThan{Float64},
    MOI.LessThan{Float64},
    MOI.EqualTo{Float64},
}

const VECTOR_SETS = Union{
    MOI.SecondOrderCone,
    MOI.RotatedSecondOrderCone,
}

# dual variable info
mutable struct ConstraintInfo{T<:Union{Float64, Vector{Float64}}}
    start::T
    upper::T
    lower::T
    function ConstraintInfo{Float64}()
        new(NaN, NaN, NaN)
    end
    function ConstraintInfo{Vector{Float64}}(N::Integer)
        new(fill(NaN, N), fill(NaN, N), fill(NaN, N))
    end
end

mutable struct VariableInfo{T<:Union{Float64, Vector{Float64}}}
    upper::T
    lower::T
    function VariableInfo()
        new{Float64}(NaN, NaN)
    end
    function VariableInfo(N::Integer)
        new{Vector{Float64}}(fill(NaN, N), fill(NaN, N))
    end
end
function VariableInfo(_info::ConstraintInfo{T}) where T
    if isa(_info.upper, Number)
        info = VariableInfo()
        info.lower = _info.lower
        info.upper = _info.upper
    else
        info = VariableInfo(length(_info.upper))
        info.lower .= _info.lower
        info.upper .= _info.upper
    end
    return info
end

struct Complement#{M1 <: MOI.ModelLike, M2 <: MOI.ModelLike, F, S}
    is_vec
    # primal::M1
    constraint
    func_w_cte#::F
    set_w_zero#::S
    # dual::M2
    variable#::VI
    # var_set#::S2
end

abstract type AbstractBilevelSolverMode{T} end

mutable struct NoMode{T} <: AbstractBilevelSolverMode{T}
end

mutable struct SOS1Mode{T} <: AbstractBilevelSolverMode{T}
    function SOS1Mode()
        return new{Float64}()
    end
end

mutable struct ComplementMode{T} <: AbstractBilevelSolverMode{T}
    with_slack::Bool
    function ComplementMode(;with_slack = false)
        return new{Float64}(with_slack)
    end
end

mutable struct ProductMode{T} <: AbstractBilevelSolverMode{T}
    epsilon::T
    with_slack::Bool
    function ProductMode(eps::T=zero(Float64); with_slack = false) where T
        return new{Float64}(eps, with_slack)
    end
end

@enum IndicatorSetting ZERO_ONE ZERO_ZERO ONE_ONE

mutable struct IndicatorMode{T} <: AbstractBilevelSolverMode{T}
    mode::IndicatorSetting
    function IndicatorMode()
        return new{Float64}(ONE_ONE)
    end
end

struct ComplementBoundCache
    # internal usage
    upper::Dict{VI, VariableInfo}
    lower::Dict{VI, VariableInfo}
    ldual::Dict{CI, ConstraintInfo}
    # full map
    map::Dict{VI, VariableInfo}
    function ComplementBoundCache()
        return new(
            Dict{VI, VariableInfo}(),
            Dict{VI, VariableInfo}(),
            Dict{CI, ConstraintInfo}(),
            Dict{VI, VariableInfo}(),
        )
    end
end

abstract type AbstractBoundedMode{T} <: AbstractBilevelSolverMode{T} end

mutable struct FortunyAmatMcCarlMode{T} <: AbstractBoundedMode{T}
    with_slack::Bool
    primal_big_M::Float64
    dual_big_M::Float64
    cache::ComplementBoundCache
    function FortunyAmatMcCarlMode(;with_slack = false,
        primal_big_M = Inf, dual_big_M = Inf)
        return new{Float64}(
            with_slack,
            primal_big_M,
            dual_big_M,
            ComplementBoundCache()
        )
    end
end

mutable struct MixedMode{T} <: AbstractBoundedMode{T}
    default::AbstractBilevelSolverMode{T}
    constraint_mode_map_c::Dict{CI, AbstractBilevelSolverMode{T}}
    constraint_mode_map_v::Dict{VI, AbstractBilevelSolverMode{T}}
    cache::ComplementBoundCache
    function MixedMode(;default = SOS1Mode())
        return new{Float64}(
            default,
            Dict{CI, AbstractBilevelSolverMode{Float64}}(),
            Dict{VI, AbstractBilevelSolverMode{Float64}}(),
            ComplementBoundCache()
        )
    end
end

function reset!(::AbstractBilevelSolverMode)
    return nothing
end
function reset!(mode::AbstractBoundedMode)
    mode.cache = ComplementBoundCache()
    return nothing
end

function appush!(col, element::AbstractVector)
    append!(col, element)
    return nothing
end
function appush!(col, element)
    push!(col, element)
    return nothing
end

function build_full_map!(mode,
    upper_idxmap, lower_idxmap, lower_dual_idxmap, lower_primal_dual_map)
    return nothing
end
function build_full_map!(mode::AbstractBoundedMode,
        upper_idxmap, lower_idxmap, lower_dual_idxmap, lower_primal_dual_map)
    _build_bound_map!(mode.cache,
        upper_idxmap, lower_idxmap, lower_dual_idxmap, lower_primal_dual_map)
    return nothing
end
function _build_bound_map!(mode::ComplementBoundCache,
    upper_idxmap, lower_idxmap, lower_dual_idxmap, lower_primal_dual_map)
    empty!(mode.map)
    for (k,v) in mode.upper
        mode.map[upper_idxmap[k]] = v
    end
    for (k,v) in mode.lower
        mode.map[lower_idxmap[k]] = v
    end
    for (k,v) in mode.ldual
        vec = lower_primal_dual_map.primal_con_dual_var[k]#[1] # TODO check this scalar
        for var in vec
            mode.map[lower_dual_idxmap[var]] = VariableInfo(v)
        end
    end
    return nothing
end

mutable struct StrongDualityMode{T} <: AbstractBilevelSolverMode{T}
    inequality::Bool
    epsilon::T
    function StrongDualityMode(eps::T=zero(Float64); inequality = true) where T
        return new{T}(inequality, eps)
    end
end

ignore_dual_objective(::AbstractBilevelSolverMode{T}) where T = true
ignore_dual_objective(::StrongDualityMode{T}) where T = false

function accept_vector_set(mode::AbstractBilevelSolverMode{T}, con::Complement) where T
    if con.is_vec
        error("Set $(typeof(con.set_w_zero)) is not accepted when solution method is $(typeof(mode))")
    end
    return nothing
end
accept_vector_set(::ProductMode{T}, ::Complement) where T = nothing
accept_vector_set(::MixedMode{T}, ::Complement) where T = nothing

function get_canonical_complements(primal_model, primal_dual_map)
    map = primal_dual_map.primal_con_dual_var
    out = Complement[]
    for ci in keys(map)
        con = get_canonical_complement(primal_model, map, ci)
        push!(out, con)
    end
    return out
end
function get_canonical_complement(primal_model, map,
    ci::CI{F,S}) where {F, S<:VECTOR_SETS}
    T = Float64
    func = MOI.copy(MOI.get(primal_model, MOI.ConstraintFunction(), ci))::F
    set = MOI.copy(MOI.get(primal_model, MOI.ConstraintSet(), ci))::S
    dim = MOI.dimension(set)
    # vector sets have no constant
    # for i in 1:dim
    #     func.constant[i] = Dualization.set_dot(i, set, T) *
    #         Dualization.get_scalar_term(primal_model, i, ci)
    # end
    # todo - set dot on function
    con = Complement(true, ci, func, set_with_zero(set), map[ci])
    return con
end
function get_canonical_complement(primal_model, map,
    ci::CI{F,S}) where {F, S<:SCALAR_SETS}
    T = Float64
    func = MOI.copy(MOI.get(primal_model, MOI.ConstraintFunction(), ci))::F
    set = MOI.copy(MOI.get(primal_model, MOI.ConstraintSet(), ci))::S
    constant = Dualization.set_dot(1, set, T) *
        Dualization.get_scalar_term(primal_model, 1, ci)
    if F == MOI.SingleVariable
        func = MOIU.operate(+, T, func, constant)
    else
        func.constant = constant
    end
    # todo - set dot on function
    con = Complement(false, ci, func, set_with_zero(set), map[ci][1])
    return con
end

function set_with_zero(set::S) where {S<:SCALAR_SETS}
    return S(0.0)
end
function set_with_zero(set)
    return MOI.copy(set)
end

function build_bilevel(
    upper::MOI.ModelLike, lower::MOI.ModelLike,
    link::Dict{VI,VI}, upper_variables::Vector{VI},
    mode,
    upper_var_lower_ctr::Dict{VI,CI} = Dict{VI,CI}();
    copy_names::Bool = false,
    pass_start::Bool = false
    )

    # Start with an empty problem
    moi_mode = MOIU.AUTOMATIC
    m = MOIU.CachingOptimizer(MOIU.UniversalFallback(MOIU.Model{Float64}()), moi_mode)

    #=
        Initialize Lower DUAL level model
    =#
    # dualize the second level
    dual_problem = Dualization.dualize(lower,
        dual_names = DualNames("dual_","dual_"),
        variable_parameters = upper_variables,
        ignore_objective = ignore_dual_objective(mode))
    lower_dual = dual_problem.dual_model
    lower_primal_dual_map = dual_problem.primal_dual_map

    #=
        Pass Upper level model
    =#

    # key are from src, value are from dest
    upper_idxmap = MOIU.default_copy_to(m, upper, copy_names)
    if copy_names
        pass_names(m, upper, upper_idxmap)
    end

    #=
        Pass Lower level model
    =#

    handle_lower_objective_sense(lower)

    # cache and delete lower objective
    if !ignore_dual_objective(mode)
        # get primal obj
        tp_primal_obj = MOI.get(lower, MOI.ObjectiveFunctionType())
        @assert tp_primal_obj !== nothing
        lower_primal_obj = MOI.get(lower, MOI.ObjectiveFunction{tp_primal_obj}())
        # deepcopy and delete dual obj
        # MOI.set(lower, MOI.ObjectiveFunction{MOI.ScalarAffineFunction{Float64}}(), MOI.ScalarAffineFunction(MOI.ScalarAffineTerm{Float64}[], 0.0))
    end

    # initialize map to lower level model
    lower_idxmap = MOIU.IndexMap()
    for (upper_key, lower_val) in link
        lower_idxmap[lower_val] = upper_idxmap[upper_key]
    end

    # append the second level primal
    append_to(m, lower, lower_idxmap, copy_names, allow_single_bounds = true)
    if copy_names
        pass_names(m, lower, lower_idxmap)
    end

    #=
        Pass Dual of Lower level model
    =#

    # initialize map to lower level model
    if !ignore_dual_objective(mode)
        # get dual obj
        tp_dual_obj = MOI.get(lower_dual, MOI.ObjectiveFunctionType())
        @assert tp_dual_obj !== nothing
        lower_dual_obj = MOI.get(lower_dual, MOI.ObjectiveFunction{tp_dual_obj}())
        # delete dual obj
        # MOI.set(lower_dual, MOI.ObjectiveFunction{MOI.ScalarAffineFunction{Float64}}(), MOI.ScalarAffineFunction(MOI.ScalarAffineTerm{Float64}[], 0.0))
    end

    # initialize map to lower level model
    lower_dual_idxmap = MOIU.IndexMap()
    # for QP's there are dual variable that are tied to:
    # primal variables
    for (lower_primal_var_key, lower_dual_quad_slack_val) in lower_primal_dual_map.primal_var_dual_quad_slack
        lower_dual_idxmap[lower_dual_quad_slack_val] = lower_idxmap[lower_primal_var_key]
    end
    # and to upper level variable which are lower level parameters
    for (lower_primal_param_key, lower_dual_param_val) in lower_primal_dual_map.primal_parameter
        lower_dual_idxmap[lower_dual_param_val] = lower_idxmap[lower_primal_param_key]
    end
    # Dual variables might appear in the upper level
    for (upper_var, lower_con) in upper_var_lower_ctr
        var = lower_primal_dual_map.primal_con_dual_var[lower_con][1] # TODO check this scalar
        lower_dual_idxmap[var] = upper_idxmap[upper_var]
    end

    # append the second level dual
    append_to(m, lower_dual, lower_dual_idxmap, copy_names)
    if copy_names
        pass_names(m, lower_dual, lower_dual_idxmap)
    end

    #=
        Additional Optimiality conditions (to complete the KKT)
    =#

    # build map bound map for FortunyAmatMcCarlMode
    build_full_map!(mode,
    upper_idxmap, lower_idxmap, lower_dual_idxmap, lower_primal_dual_map)

    if ignore_dual_objective(mode)
        # complementary slackness
        comps = get_canonical_complements(lower, lower_primal_dual_map)
        for comp in comps
            if !is_equality(comp.set_w_zero)
                accept_vector_set(mode, comp)
                add_complement(mode, m, comp,
                    lower_idxmap, lower_dual_idxmap, copy_names, pass_start)
            else
                # println("eq in complement")
            end
        end
    else
        # strong duality
        lower_dual_obj
        lower_primal_obj
        add_strong_duality(mode, m, lower_primal_obj, lower_dual_obj, lower_idxmap, lower_dual_idxmap)
    end

    return m, upper_idxmap, lower_idxmap, lower_primal_dual_map, lower_dual_idxmap
end

function add_strong_duality(mode::StrongDualityMode{T}, m, primal_obj, dual_obj,
    idxmap_primal, idxmap_dual) where T

    primal = MOIU.map_indices.(Ref(idxmap_primal), primal_obj)
    dual   = MOIU.map_indices.(Ref(idxmap_dual), dual_obj)

    func = MOIU.operate(-, T, primal, dual)

    if !mode.inequality
        c = MOIU.normalize_and_add_constraint(m, func, MOI.EqualTo(zero(T)))
        MOI.set(m, MOI.ConstraintName(), c, "lower_strong_duality")
        return CI[c]
    else
        func_up = MOIU.operate(-, T, func, mode.epsilon)
        c_up = MOIU.normalize_and_add_constraint(m, func_up, MOI.LessThan(zero(T)))
        MOI.set(m, MOI.ConstraintName(), c_up, "lower_strong_duality_up")

        func_lo = MOIU.operate(+, T, func, mode.epsilon)
        c_lo = MOIU.normalize_and_add_constraint(m, func_lo, MOI.GreaterThan(zero(T)))
        MOI.set(m, MOI.ConstraintName(), c_lo, "lower_strong_duality_lo")

        return CI[c_up, c_lo]
    end
end

function add_complement(mode::MixedMode{T}, m, comp::Complement,
        idxmap_primal, idxmap_dual, copy_names::Bool, pass_start::Bool) where T
    _mode = get_mode(mode, comp.constraint, idxmap_primal)
    accept_vector_set(_mode, comp)
    add_complement(_mode, m, comp,
        idxmap_primal, idxmap_dual, copy_names, pass_start)
end

function get_mode(mode::MixedMode{T}, ci::CI{F,S}, map) where {
    T,
    F<:MOI.SingleVariable,
    S<:Union{MOI.EqualTo{T}, MOI.LessThan{T}, MOI.GreaterThan{T}}
}
    # key = map[VI(ci.value)]
    key = VI(ci.value)
    if haskey(mode.constraint_mode_map_v, key)
        return mode.constraint_mode_map_v[key]
    else
        return mode.default
    end
end
function get_mode(mode::MixedMode{T}, ci::CI{F,S}, map) where T where {F,S}
    key = ci#map[ci]
    # @show ci, key, map
    # @show mode.constraint_mode_map_c
    if haskey(mode.constraint_mode_map_c, key)
        return mode.constraint_mode_map_c[key]
    else
        return mode.default
    end
end

function add_complement(mode::ComplementMode{T}, m, comp::Complement,
    idxmap_primal, idxmap_dual, copy_names::Bool, pass_start::Bool) where T
    f = comp.func_w_cte
    s = comp.set_w_zero
    v = comp.variable
    out_var = VI[]
    out_ctr = CI[]

    with_slack = mode.with_slack

    f_dest = MOIU.map_indices.(Ref(idxmap_primal), f)

    dual = idxmap_dual[v]

    if with_slack
        slack, slack_in_set = MOI.add_constrained_variable(m, s)
        new_f = MOIU.operate(-, T, f_dest, MOI.SingleVariable(slack))
        equality = MOIU.normalize_and_add_constraint(m, new_f, MOI.EqualTo(zero(T)))

        if pass_start
            val = MOIU.eval_variables(
                x-> nothing_to_nan(MOI.get(m, MOI.VariablePrimalStart(), x)), f_dest)
            if !isnan(val)
                MOI.set(m, MOI.VariablePrimalStart(), slack, val)
            end
        end

        c = MOI.add_constraint(m,
            MOI.VectorOfVariables([slack, dual]),
            MOI.Complements(1))
        if copy_names
            nm = MOI.get(m, MOI.VariableName(), dual)
            MOI.set(m, MOI.VariableName(), slack, "slk_($(nm))")
            MOI.set(m, MOI.ConstraintName(), slack_in_set, "bound_slk_($(nm))")
            MOI.set(m, MOI.ConstraintName(), equality, "eq_slk_($(nm))")
            MOI.set(m, MOI.ConstraintName(), c, "compl_complWslk_($(nm))")
        end

        appush!(out_var, slack)
        appush!(out_ctr, slack_in_set)
        appush!(out_ctr, equality)
        appush!(out_ctr, c)
    else
        new_f = MOIU.operate(vcat, T, f_dest, MOI.SingleVariable(dual))

        c = MOI.add_constraint(m,
            new_f,
            MOI.Complements(1))

        if copy_names
            nm = MOI.get(m, MOI.VariableName(), dual)
            MOI.set(m, MOI.ConstraintName(), c, "compl_compl_($(nm))")
        end

        appush!(out_ctr, c)
    end

    return out_var, out_ctr
end

function add_complement(mode::SOS1Mode{T}, m, comp::Complement,
    idxmap_primal, idxmap_dual, copy_names::Bool, pass_start::Bool) where T
    f = comp.func_w_cte
    s = comp.set_w_zero
    v = comp.variable

    if comp.is_vec
        error("Vector constraint is not supported by SOS1 mode")
    end

    slack, slack_in_set = MOI.add_constrained_variable(m, s)
    f_dest = MOIU.map_indices.(Ref(idxmap_primal), f)
    new_f = MOIU.operate(-, T, f_dest, MOI.SingleVariable(slack))
    equality = MOIU.normalize_and_add_constraint(m, new_f, MOI.EqualTo(zero(T)))

    dual = idxmap_dual[v]
    c1 = MOI.add_constraint(m,
        MOI.VectorOfVariables([slack, dual]),
        MOI.SOS1([1.0, 2.0]))

    if copy_names
        nm = MOI.get(m, MOI.VariableName(), dual)
        MOI.set(m, MOI.VariableName(), slack, "slk_($(nm))")
        MOI.set(m, MOI.ConstraintName(), slack_in_set, "bound_slk_($(nm))")
        MOI.set(m, MOI.ConstraintName(), equality, "eq_slk_($(nm))")
        MOI.set(m, MOI.ConstraintName(), c1, "compl_sos1_($(nm))")
    end

    return slack, slack_in_set, equality, c1
end

is_equality(set::S) where {S<:MOI.AbstractSet} = false
is_equality(set::MOI.EqualTo{T}) where T = true
is_equality(set::MOI.Zeros) = true

only_variable_functions(v::MOI.VariableIndex) = MOI.SingleVariable(v)
only_variable_functions(v::Vector{MOI.VariableIndex}) = MOI.VectorOfVariables(v)

nothing_to_nan(val) = ifelse(val === nothing, NaN, val)

function add_complement(mode::ProductMode{T}, m, comp::Complement,
    idxmap_primal, idxmap_dual, copy_names::Bool, pass_start::Bool) where T
    f = comp.func_w_cte
    s = comp.set_w_zero
    v = comp.variable

    out_var = VI[]
    out_ctr = CI[]

    eps = mode.epsilon
    with_slack = mode.with_slack

    f_dest = MOIU.map_indices(x->idxmap_primal[x], f)

    dual = comp.is_vec ? map(x->idxmap_dual[x], v) : idxmap_dual[v]

    if with_slack
        slack, slack_in_set = if comp.is_vec
            MOI.add_constrained_variables(m, s)
        else
            MOI.add_constrained_variable(m, s)
        end
        new_f = MOIU.operate(-, T, f_dest, only_variable_functions(slack))
        if comp.is_vec
            equality = MOIU.normalize_and_add_constraint(m, new_f, MOI.Zeros(length(slack)))
        else
            equality = MOIU.normalize_and_add_constraint(m, new_f, MOI.EqualTo(zero(T)))
        end

        prod_f = MOIU.operate(dot, T, only_variable_functions(slack), only_variable_functions(dual))

        prod_f1 = MOIU.operate(-, T, prod_f, eps)
        c1 = MOIU.normalize_and_add_constraint(m,
            prod_f1,
            MOI.LessThan{Float64}(0.0))
        if comp.is_vec
            prod_f2 = MOIU.operate(+, T, prod_f, eps)
            c2 = MOIU.normalize_and_add_constraint(m,
                prod_f2,
                MOI.GreaterThan{Float64}(0.0))
        end

        appush!(out_var, slack)
        appush!(out_ctr, slack_in_set)
        appush!(out_ctr, equality)
        appush!(out_ctr, c1)
        if comp.is_vec
            appush!(out_ctr, c2)
        end

        if pass_start
            val = MOIU.eval_variables(
                x-> nothing_to_nan(MOI.get(m, MOI.VariablePrimalStart(), x)), f_dest)
            if comp.is_vec
                for i in eachindex(val)
                    if !isnan(val[i])
                        MOI.set(m, MOI.VariablePrimalStart(), slack[i], val[i])
                    end
                end
            else
                if !isnan(val)
                    MOI.set(m, MOI.VariablePrimalStart(), slack, val)
                end
            end
        end

        if copy_names
            nm = MOI.get(m, MOI.VariableName(), dual)
            MOI.set(m, MOI.VariableName(), slack, "slk_($(nm))")
            MOI.set(m, MOI.ConstraintName(), slack_in_set, "bound_slk_($(nm))")
            MOI.set(m, MOI.ConstraintName(), equality, "eq_slk_($(nm))")
            MOI.set(m, MOI.ConstraintName(), c1, "compl_prodWslk_($(nm))")
            if comp.is_vec
                MOI.set(m, MOI.ConstraintName(), c1, "compl_prodWslk2_($(nm))")
            end
        end
    else
        new_f = MOIU.operate(dot, T, f_dest, only_variable_functions(dual))
        new_f1 = MOIU.operate(-, T, new_f, eps)
        c1 = MOIU.normalize_and_add_constraint(m,
            new_f1,
            MOI.LessThan{T}(0.0))
        if comp.is_vec # conic
            new_f2 = MOIU.operate(+, T, new_f, eps)
            c2 = MOIU.normalize_and_add_constraint(m,
                new_f2,
                MOI.GreaterThan{T}(0.0))
        end

        # TODO(?): if eps == 0 then add equality

        appush!(out_ctr, c1)
        if comp.is_vec
            appush!(out_ctr, c2)
        end

        if copy_names
            nm = if comp.is_vec
                MOI.get.(m, MOI.VariableName(), dual)
            else
                MOI.get(m, MOI.VariableName(), dual)
            end
            MOI.set(m, MOI.ConstraintName(), c1, "compl_prod_($(nm))")
            if comp.is_vec
                MOI.set(m, MOI.ConstraintName(), c2, "compl_prod2_($(nm))")
            end
        end
    end
    return out_var, out_ctr
end

function add_complement(mode::IndicatorMode{T}, m, comp::Complement,
    idxmap_primal, idxmap_dual, copy_names::Bool, pass_start::Bool) where T
    f = comp.func_w_cte
    s = comp.set_w_zero
    v = comp.variable

    method = mode.mode

    is_tight = false
    has_start = false

    f_dest = MOIU.map_indices(x->idxmap_primal[x], f)

    dual = idxmap_dual[v]

    if comp.is_vec
        error("Vector constraint is (currently) not supported by indicator mode")
    end

    if copy_names
        nm = if comp.is_vec
            MOI.get.(m, MOI.VariableName(), dual)
        else
            MOI.get(m, MOI.VariableName(), dual)
        end
    end

    vb1 = MOI.add_variable(m)
    if copy_names
        MOI.set(m, MOI.VariableName(), vb1, "compl_bin1_($(nm))")
    end

    cb1 = MOI.add_constraint(m, SVF(vb1), MOI.ZeroOne())
    if method == ONE_ONE || method == ZERO_ZERO
        # second binary
        vb2 = MOI.add_variable(m)
        cb2 = MOI.add_constraint(m, SVF(vb2), MOI.ZeroOne())
        if copy_names
            MOI.set(m, MOI.VariableName(), vb2, "compl_bin2_($(nm))")
        end

        # z1 + z2 == 1
        fb = MOIU.operate(+, T, SVF(vb1), SVF(vb2))
        cb = MOI.add_constraint(m, fb, MOI.EqualTo{T}(one(T)))
        if copy_names
            MOI.set(m, MOI.ConstraintName(), cb, "compl_sum_bin_($(nm))")
        end
    else
        vb2 = vb1
    end

    pre_f1, pre_s1 = MOIU.normalize_constant(f_dest, MOI.EqualTo(zero(T)))
    f1 = MOIU.operate(vcat, T, SVF(vb1), pre_f1)
    f2 = MOIU.operate(vcat, T, SVF(vb2), SVF(dual))

    if pass_start
        val = MOIU.eval_variables(
            x-> nothing_to_nan(MOI.get(m, MOI.VariablePrimalStart(), x)), f_dest)
        if !isnan(val)
            is_tight = abs(val) < 1e-8
            has_start = true
        end
    end

    if method == ONE_ONE
        s1 = MOI.IndicatorSet{MOI.ACTIVATE_ON_ONE}(pre_s1)
        s2 = MOI.IndicatorSet{MOI.ACTIVATE_ON_ONE}(MOI.EqualTo(zero(T)))
        if pass_start && has_start
            MOI.set(m, MOI.VariablePrimalStart(), vb1, ifelse(is_tight, 1.0, 0.0))
            MOI.set(m, MOI.VariablePrimalStart(), vb2, ifelse(is_tight, 0.0, 1.0))
        end
    elseif method == ZERO_ZERO
        s1 = MOI.IndicatorSet{MOI.ACTIVATE_ON_ZERO}(pre_s1)
        s2 = MOI.IndicatorSet{MOI.ACTIVATE_ON_ZERO}(MOI.EqualTo(zero(T)))
        if pass_start && has_start
            MOI.set(m, MOI.VariablePrimalStart(), vb1, ifelse(is_tight, 0.0, 1.0))
            MOI.set(m, MOI.VariablePrimalStart(), vb2, ifelse(is_tight, 1.0, 0.0))
        end
    else
        s1 = MOI.IndicatorSet{MOI.ACTIVATE_ON_ONE}(pre_s1)
        s2 = MOI.IndicatorSet{MOI.ACTIVATE_ON_ZERO}(MOI.EqualTo(zero(T)))
        if pass_start && has_start
            MOI.set(m, MOI.VariablePrimalStart(), vb1, ifelse(is_tight, 1.0, 0.0))
        end
    end

    # do not MOIU.normalize_and_add_constraint because are vector functions
    c1 = MOI.add_constraint(m, to_vector_affine(f1), s1)
    c2 = MOI.add_constraint(m, to_vector_affine(f2), s2)

    if copy_names
        MOI.set(m, MOI.ConstraintName(), c1, "compl_ind1_($(nm))")
        MOI.set(m, MOI.ConstraintName(), c2, "compl_ind2_($(nm))")
    end
    return c1
end

function flip_set(set::MOI.LessThan{T}) where T
    return MOI.GreaterThan{T}(0.0)
end
function flip_set(set::MOI.GreaterThan{T}) where T
    return MOI.LessThan{T}(0.0)
end

function get_bounds(var, map, fallback_bound = Inf)
    if haskey(map, var)
        info = map[var]
        # TODO deal with precision and performance
        lower = ifelse(info.lower != -Inf, info.lower, -fallback_bound)
        upper = ifelse(info.upper != +Inf, info.upper, +fallback_bound)
        return IntervalArithmetic.interval(lower, upper)
    elseif 0.0 <= fallback_bound <= Inf
        return IntervalArithmetic.interval(-fallback_bound, fallback_bound)
    else
        error("variable $var has no finite bounds defined")
    end
end

function set_bound(inter::IntervalArithmetic.Interval, ::LT{T}) where T
    return inter.hi
end
function set_bound(inter::IntervalArithmetic.Interval, ::GT{T}) where T
    return inter.lo
end

function add_complement(mode::FortunyAmatMcCarlMode{T}, m, comp::Complement,
    idxmap_primal, idxmap_dual, copy_names::Bool, pass_start::Bool) where T

    f = comp.func_w_cte
    s = comp.set_w_zero
    v = comp.variable

    is_tight = false
    has_start = false

    if mode.with_slack
        slack, slack_in_set = MOI.add_constrained_variable(m, s)
    end
    f_dest = MOIU.map_indices.(Ref(idxmap_primal), f)

    f_bounds = MOIU.eval_variables(vi -> get_bounds(vi, mode.cache.map, mode.primal_big_M), f_dest)

    if pass_start
        val = MOIU.eval_variables(
            x-> nothing_to_nan(MOI.get(m, MOI.VariablePrimalStart(), x)), f_dest)
        if !isnan(val)
            is_tight = abs(val) < 1e-8
            has_start = true
        end
    end

    if mode.with_slack
        new_f = MOIU.operate(-, T, f_dest, MOI.SingleVariable(slack))
        equality = MOIU.normalize_and_add_constraint(m, new_f, MOI.EqualTo(zero(T)))
        if pass_start && has_start
            MOI.set(m, MOI.VariablePrimalStart(), slack, val)
        end
    end

    dual = idxmap_dual[v]
    v_bounds = get_bounds(dual, mode.cache.map, mode.dual_big_M)

    bin = MOI.add_variable(m)
    if pass_start && has_start && is_tight
        MOI.set(m, MOI.VariablePrimalStart(), bin, 1.0)
        MOI.set(m, MOI.VariablePrimalStart(), dual, 0.0)
    else
        MOI.set(m, MOI.VariablePrimalStart(), bin, 0.0)
    end

    s1 = flip_set(s)
    s2 = flip_set(s)

    Ms = set_bound(f_bounds, s1)
    Mv = set_bound(v_bounds, s2)

    if isnan(Ms) || abs(Ms) >= Inf || isnan(Mv) || abs(Mv) >= Inf
        error("It was not possible to automatically compute bounds"*
            " for a complementarity pair, please add the arguments"*
            " primal_big_M and dual_big_M to FortunyAmatMcCarlMode")
    end

    if mode.with_slack
        f1 = MOI.ScalarAffineFunction{T}(
            MOI.ScalarAffineTerm{T}.(
                [one(T), -Ms], [slack, bin]
            ),
            0.0
        )
    else
        push!(f_dest.terms, MOI.ScalarAffineTerm{T}(-Ms, bin))
        f1 = f_dest
    end

    f2 = MOI.ScalarAffineFunction{T}(
        MOI.ScalarAffineTerm{T}.(
            [one(T), Mv], [dual, bin]
        ),
        -Mv
    )

    c1 = MOIU.normalize_and_add_constraint(m, f1, s2)
    c2 = MOIU.normalize_and_add_constraint(m, f2, s2)
    c3 = MOI.add_constraint(m, MOI.SingleVariable(bin), MOI.ZeroOne())

    if copy_names
        nm = MOI.get(m, MOI.VariableName(), dual)
        if mode.with_slack
            MOI.set(m, MOI.VariableName(), slack, "slk_($(nm))")
            MOI.set(m, MOI.ConstraintName(), slack_in_set, "bound_slk_($(nm))")
            MOI.set(m, MOI.ConstraintName(), equality, "eq_slk_($(nm))")
        end
        MOI.set(m, MOI.VariableName(), bin, "bin_($(nm))")
        MOI.set(m, MOI.ConstraintName(), c1, "compl_fa_sl_($(nm))")
        MOI.set(m, MOI.ConstraintName(), c2, "compl_fa_dl_($(nm))")
        MOI.set(m, MOI.ConstraintName(), c2, "compl_fa_bn_($(nm))")
    end

    # if mode.with_slack
    #     return slack, slack_in_set, equality, c1
    # else
    # end
    return c1
end

function to_vector_affine(f::MOI.VectorAffineFunction{T}) where T
    return f
end
function to_vector_affine(f::MOI.VectorOfVariables)
    return MOI.VectorAffineFunction{Float64}(f)
end

function handle_lower_objective_sense(lower::MOI.ModelLike)
    lower_objective_sense = MOI.get(lower, MOI.ObjectiveSense())
    if lower_objective_sense == MOI.FEASIBILITY_SENSE
        throw(ErrorException("Lower level models with objective_sense: " *
                            lower_objective_sense *
                            " are not supported."))
    end
    return
end


__precompile__()

module TMCM3110

export encode_command
function encode_command(m_address, n_command, n_type, n_motor, value)
  m_address = UInt8( m_address % (1<<8) )
  n_command = UInt8( n_command % (1<<8) )
  n_type    = UInt8( n_type % (1<<8)    )
  n_motor   = UInt8( n_motor % (1<<8)   )
  value     = Int32( value )
  values = [ parse(UInt8, bits(value)[ 1+(8*(i-1)) : 8+(8*(i-1)) ] , 2) for i in 1:4]

  checksum  = UInt8( (m_address + n_command + n_type + n_motor + sum(values)) % (1<<8) )

  tmcl_bytes = [m_address, n_command, n_type, n_motor, values..., checksum]

  return tmcl_bytes
end

export decode_reply
function decode_reply(reply)
  r_address  = UInt8( reply[1] )
  m_address  = UInt8( reply[2] )
  r_status   = UInt8( reply[3] )
  Int32(r_status) != 100 ? warn(STATUSCODES[Int(r_status)]) : nothing
  n_command  = UInt8( reply[4] )
  values     = [ UInt8(value) for value in reply[5:8] ]
  r_checksum = UInt8( reply[9] )
  checksum = UInt8( (r_address + m_address + r_status + n_command + sum(values)) % (1<<8) )
  value      = parse( UInt32, "$(bits(values[1]))$(bits(values[2]))$(bits(values[3]))$(bits(values[4]))" , 2 )
  value      = reinterpret(Int32, value)
  return value
end

export STATUSCODES
STATUSCODES = Dict( 100 => "Succesfully executed, no error",
                    101 => "Command loaded into TMCL program EEPROM",
                      1 => "Wrong Checksum",
                      2 => "Invalid command",
                      3 => "Wrong type",
                      4 => "Invalid value",
                      5 => "Configuration EEPROM locked",
                      6 => "Command not available" )

export COMMAND_NUMBERS
COMMAND_NUMBERS = Dict(  1 => "ROR",
                         2 => "ROL",
                         3 => "MST",
                         4 => "MVP",
                         5 => "SAP",
                         6 => "GAP",
                         7 => "STAP",
                         8 => "RSAP",
                         9 => "SGP",
                        10 => "GGP",
                        11 => "STGP",
                        12 => "RSGP",
                        13 => "RFS",
                        14 => "SIO",
                        15 => "GIO",
                        19 => "CALC",
                        20 => "COMP",
                        21 => "JC",
                        22 => "JA",
                        23 => "CSUB",
                        24 => "RSUB",
                        25 => "EI",
                        26 => "DI",
                        27 => "WAIT",
                        28 => "STOP",
                        30 => "SCO",
                        31 => "GCO",
                        32 => "CCO",
                        33 => "CALCX",
                        34 => "AAP",
                        35 => "AGP",
                        37 => "VECT",
                        38 => "RETI",
                        39 => "ACO"  )

export AXIS_PARAMETER
AXIS_PARAMETER = Dict(   0 => "target position",
                         1 => "actual position",
                         2 => "target speed",
                         3 => "actual speed",
                         4 => "max positioning speed",
                         5 => "max acceleration",
                         6 => "abs max current",
                         7 => "standby current",
                         8 => "target pos reached",
                         9 => "ref switch status",
                        10 => "right limit switch status",
                        11 => "left limit switch status",
                        12 => "right limit switch disable",
                        13 => "left limit switch disable",
                       130 => "minimum speed",
                       135 => "actual acceleration",
                       138 => "ramp mode",
                       140 => "microstep resolution",
                       141 => "ref switch tolerance",
                       149 => "soft stop flag",
                       153 => "ramp divisor",
                       154 => "pulse divisor",
                       160 => "step interpolation enable",
                       161 => "double step enable",
                       162 => "chopper blank time",
                       163 => "chopper mode",
                       164 => "chopper hysteresis dec",
                       165 => "chopper hysteresis end",
                       166 => "chopper hysteresis start",
                       167 => "chopper off time",
                       168 => "smartEnergy min current",
                       169 => "smartEnergy current downstep",
                       170 => "smartEnergy hysteresis",
                       171 => "smartEnergy current upstep",
                       172 => "smartEnergy hysteresis start",
                       173 => "stallGuard2 filter enable",
                       174 => "stallGuard2 threshold",
                       175 => "slope control high side",
                       176 => "slope control low side",
                       177 => "short protection disable",
                       178 => "short detection timer",
                       179 => "Vsense",
                       180 => "smartEnergy actual current",
                       181 => "stop on stall",
                       182 => "smartEnergy threshold speed",
                       183 => "smartEnergy slow run current",
                       193 => "ref. search mode",
                       194 => "ref. search speed",
                       195 => "ref. switch speed",
                       196 => "distance end switches",
                       204 => "freewheeling",
                       206 => "actual load value",
                       208 => "TMC262 errorflags",
                       209 => "encoder pos",
                       210 => "encoder prescaler",
                       212 => "encoder max deviation",
                       214 => "power down delay"    )

export INTERRUPT_VECTORS
INTERRUPT_VECTORS = Dict(  0 => "Timer 0",
                           1 => "Timer 1",
                           2 => "Timer 2",
                           3 => "Target position 0 reached",
                           4 => "Target position 1 reached",
                           5 => "Target position 2 reached",
                          15 => "stallGuard2 axis 0",
                          16 => "stallGuard2 axis 1",
                          17 => "stallGuard2 axis 2",
                          21 => "Deviation axis 0",
                          22 => "Deviation axis 1",
                          23 => "Deviation axis 2",
                          27 => "Left stop switch 0",
                          28 => "Right stop switch 0",
                          29 => "Left stop switch 1",
                          30 => "Right stop switch 1",
                          31 => "Left stop switch 2",
                          32 => "Right stop switch 2",
                          39 => "Input change 0",
                          40 => "Input change 1",
                          41 => "Input change 2",
                          42 => "Input change 3",
                          43 => "Input change 4",
                          44 => "Input change 5",
                          45 => "Input change 6",
                          46 => "Input change 7",
                         255 => "Global interrupts" )

end # module


import MelGeneralizedCepstrums: allpass_alpha, glog_gamma

abstract type Filter end
abstract type SynthesisFilter <: Filter end

### Generic interfaces ###

# Note that f has state and filt! should update state value in this function
# TODO(ryuichi): separate state and filt definition?
function filt!(f::SynthesisFilter, x::Real, coef::AbstractVector)
    error("should provide sample-by-sample filt!")
end

# A synthesis filter must provide conversion from spectral parameter
# (e.g. mel-cepstrum) to filter coefficients.
function to_filtcoef(f::SynthesisFilter, param::AbstractArray)
    error("cannot determine how to convert paramters")
end

### Interfaces for mel-generalized ceptrum synthesis filters ###

abstract type MelGeneralizedCepstrumSynthesisFilter <: SynthesisFilter end

function allpass_alpha(f::MelGeneralizedCepstrumSynthesisFilter)
    error("should provide get access to allpass constant")
end

function glog_gamma(f::MelGeneralizedCepstrumSynthesisFilter)
    error("should provide get access to glog gamma")
end

# LPC, PARCOR and LSP
abstract type LinearPredictionVariantsSynthesisFilter <: SynthesisFilter end

### Waveform synthesis implementation ###

# synthesis_one_frame! generates a one frame speech waveform given a excitation
# signal and successive filter coefficients of a synthesis filter.
function synthesis_one_frame!(f::SynthesisFilter,
                              y::AbstractVector, # result will be stored
                              excitation::AbstractVector,
                              bᵗ⁻¹::AbstractVector,
                              bᵗ::AbstractVector)
    @assert length(y) == length(excitation)
    slope = (bᵗ - bᵗ⁻¹) / length(excitation)

    interpolated_coef = copy(bᵗ⁻¹)

    for i in 1:length(excitation)
        scaled_excitation = excitation[i] * exp(interpolated_coef[1])
        y[i] = filt!(f, scaled_excitation, interpolated_coef)
        for j=1:length(slope)
            interpolated_coef[j] += slope[j]
        end
    end

    y
end

function synthesis_one_frame!(f::SynthesisFilter,
                              excitation::AbstractVector,
                              bᵗ⁻¹::AbstractVector,
                              bᵗ::AbstractVector)
    synthesis_one_frame!(f, similar(excitation), excitation, bᵗ⁻¹, bᵗ)
end

# synthesis! generates a speech waveform given a excitation signal and
# a sequence of spectral envelope parameter.
function synthesis!(f::SynthesisFilter, # filter states are will be updated
                    excitation::AbstractVector,
                    b::AbstractMatrix,
                    hopsize::Integer)
    synthesized = similar(excitation)
    fill!(synthesized, zero(eltype(synthesized)))

    bᵗ⁻¹ = b[:,1]
    bᵗ = similar(bᵗ⁻¹)
    buf = Vector{eltype(synthesized)}(hopsize)

    for i in 1:size(b, 2)
        if i > 1
            bᵗ⁻¹ = b[:,i-1]
        end
        bᵗ = b[:,i]

        s, e = (i-1)*hopsize+1, i*hopsize
        e > length(excitation) && break

        synthesis_one_frame!(f, buf, excitation[s:e], bᵗ⁻¹, bᵗ)
        copy!(synthesized, s, buf, 1, length(buf))
    end

    synthesized
end

### Dispatch on specific filter types ###

function synthesis!{T<:MelGeneralizedCepstrum}(f::MelGeneralizedCepstrumSynthesisFilter,
                    excitation::AbstractVector,
                    state::SpectralParamState{T},
                    hopsize::Integer)
    synthesis!(f, excitation, to_filtcoef(f, state), hopsize)
end

function synthesis!{T<:LinearPredictionCoefVariants}(f::LinearPredictionVariantsSynthesisFilter,
                    excitation::AbstractVector,
                    state::SpectralParamState{T},
                    hopsize::Integer)
    synthesis!(f, excitation, to_filtcoef(f, state), hopsize)
end