# MIT License
#
# Copyright (c) 2018 Martin Biel
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.

# Functions convertible to a AffineDecisionFunction
const ScalarAffineLike{T} = Union{MOIU.ScalarAffineLike{T}, SingleDecision, AffineDecisionFunction{T}}
const ScalarAffineDecisionLike{T} = Union{SingleDecision, AffineDecisionFunction{T}}

# Functions convertible to a QuadraticDecisionFunction
const ScalarQuadraticLike{T} = Union{MOIU.ScalarQuadraticLike{T}, SingleDecision, AffineDecisionFunction{T}, QuadraticDecisionFunction{T}}
const ScalarQuadraticDecisionLike{T} = Union{SingleDecision, AffineDecisionFunction{T}, QuadraticDecisionFunction{T}}

const TypedScalarDecisionLike{T} = Union{AffineDecisionFunction{T}, QuadraticDecisionFunction{T}}
const ScalarDecisionLike{T} = Union{SingleDecision, TypedScalarDecisionLike{T}}
const MixedTypedScalarLike{T} = Union{MOIU.TypedScalarLike{T}, TypedScalarDecisionLike{T}}
const MixedScalarLike{T} = Union{MOIU.ScalarLike{T}, ScalarDecisionLike{T}}

# Functions convertible to a VectorAffineDecisionFunction
const VectorAffineLike{T} = Union{MOIU.VectorAffineLike{T}, VectorOfDecisions, VectorAffineDecisionFunction{T}}
const VectorAffineDecisionLike{T} = Union{VectorOfDecisions, VectorAffineDecisionFunction{T}}

const TypedVectorDecisionLike{T} = Union{VectorAffineDecisionFunction{T}}
const VectorDecisionLike{T} = Union{VectorOfDecisions, VectorAffineDecisionFunction{T}}
const MixedVectorLike{T} = Union{MOIU.VectorLike{T}, VectorDecisionLike{T}}

const TypedDecisionLike{T} = Union{TypedScalarDecisionLike{T}, TypedVectorDecisionLike{T}}
const AffineLike{T} = Union{ScalarAffineLike{T}, VectorAffineLike{T}}
const AffineDecisionLike{T} = Union{ScalarAffineDecisionLike{T}, VectorAffineDecisionLike{T}}
const DecisionLike{T} = Union{ScalarDecisionLike{T}, VectorDecisionLike{T}}

# +/- #
# ========================== #
function MOIU.promote_operation(::typeof(-), ::Type{T},
                                ::Type{<:ScalarAffineDecisionLike{T}}) where T
    return AffineDecisionFunction{T}
end
function MOIU.promote_operation(::typeof(-), ::Type{T},
                                ::Type{<:ScalarQuadraticDecisionLike{T}}) where T
    return QuadraticDecisionFunction{T}
end
function MOIU.promote_operation(::typeof(-), ::Type{T},
                                ::Type{QuadraticDecisionFunction{T}}) where T
    return QuadraticDecisionFunction{T}
end

function MOIU.promote_operation(::Union{typeof(+), typeof(-)}, ::Type{T},
                                ::Type{<:ScalarAffineDecisionLike{T}},
                                ::Type{<:ScalarAffineLike{T}}) where T
    return AffineDecisionFunction{T}
end
function MOIU.promote_operation(::Union{typeof(+), typeof(-)}, ::Type{T},
                                ::Type{<:MOIU.ScalarAffineLike{T}},
                                ::Type{<:ScalarAffineDecisionLike{T}}) where T
    return AffineDecisionFunction{T}
end

function MOIU.promote_operation(::Union{typeof(+), typeof(-)}, ::Type{T},
                                ::Type{<:ScalarQuadraticDecisionLike{T}},
                                ::Type{<:ScalarQuadraticLike{T}}) where T
    return QuadraticDecisionFunction{T}
end
function MOIU.promote_operation(::Union{typeof(+), typeof(-)}, ::Type{T},
                                ::Type{<:MOIU.ScalarQuadraticLike{T}},
                                ::Type{<:ScalarQuadraticDecisionLike{T}}) where T
    return QuadraticDecisionFunction{T}
end

# Unary -
function MOIU.operate!(op::typeof(-), ::Type{T},
                       f::Union{AffineDecisionFunction{T},
                                QuadraticDecisionFunction{T}}) where T
    return MA.mutable_operate!(-, f)
end

# AffineDecisionFunction +/-! ...
function MOIU.operate!(op::Union{typeof(+), typeof(-)}, ::Type{T},
                       f::AffineDecisionFunction{T},
                       g::ScalarAffineLike{T}) where T
    return MA.mutable_operate!(op, f, g)
end
function MOIU.operate!(op::Union{typeof(+), typeof(-)}, ::Type{T},
                       f::SingleDecision,
                       g::ScalarQuadraticLike{T}) where T
    return MOIU.operate(op, T, f, g)
end
function MOIU.operate!(op::Union{typeof(+), typeof(-)}, ::Type{T},
                       f::AffineDecisionFunction{T},
                       g::MOI.ScalarQuadraticFunction{T}) where T
    return MOIU.operate(op, T, f, g)
end
function MOIU.operate!(op::Union{typeof(+), typeof(-)}, ::Type{T},
                       f::AffineDecisionFunction{T},
                       g::QuadraticDecisionFunction{T}) where T
    return MOIU.operate(op, T, f, g)
end
# QuadraticDecisionFunction +/-! ...
function MOIU.operate!(op::Union{typeof(+), typeof(-)}, ::Type{T},
                       f::QuadraticDecisionFunction{T},
                       g::ScalarQuadraticLike{T}) where T
    return MA.mutable_operate!(op, f, g)
end

# Scalar number +/- ...
function MOIU.operate(op::typeof(+), ::Type{T}, α::T, f::ScalarDecisionLike{T}) where T
    return MOIU.operate(op, T, f, α)
end
function MOIU.operate(op::typeof(-), ::Type{T}, α::T, f::ScalarDecisionLike{T}) where T
    return MOIU.operate!(+, T, MOIU.operate(-, T, f), α)
end

# SingleDecision +/- ...
function MOIU.operate(::typeof(-), ::Type{T}, f::SingleDecision) where T
    return AffineDecisionFunction{T}(convert(MOI.ScalarAffineFunction{T}, zero(T)),
                                     MOI.ScalarAffineFunction([MOI.ScalarAffineTerm(-one(T), f.decision)], zero(T)))
end
function MOIU.operate(op::Union{typeof(+), typeof(-)}, ::Type{T},
                      f::SingleDecision, α::T) where T
    return AffineDecisionFunction{T}(convert(MOI.ScalarAffineFunction{T}, op(α)),
                                     MOI.ScalarAffineFunction([MOI.ScalarAffineTerm(one(T), f.decision)], zero(T)))
end
function MOIU.operate(op::Union{typeof(+), typeof(-)}, ::Type{T},
                      f::SingleDecision,
                      g::MOI.SingleVariable) where T
    return AffineDecisionFunction{T}(MOI.ScalarAffineFunction([MOI.ScalarAffineTerm(one(T), g.variable)], zero(T)),
                                     MOI.ScalarAffineFunction([MOI.ScalarAffineTerm(one(T), f.decision)], zero(T)))
end
function MOIU.operate(op::Union{typeof(+), typeof(-)}, ::Type{T},
                      f::SingleDecision,
                      g::SingleDecision) where T
    return AffineDecisionFunction{T}(convert(MOI.ScalarAffineFunction{T}, zero(T)),
                                     MOI.ScalarAffineFunction(
                                         [MOI.ScalarAffineTerm(one(T), f.decision),
                                          MOI.ScalarAffineTerm(op(one(T)), g.decision)], zero(T)))
end
function MOIU.operate(op::typeof(+), ::Type{T},
                      f::SingleDecision,
                      g::MOI.ScalarAffineFunction{T}) where T
    return MOIU.operate(op, T, convert(AffineDecisionFunction{T}, g), f)
end
function MOIU.operate(op::typeof(-), ::Type{T},
                      f::SingleDecision,
                      g::MOI.ScalarAffineFunction{T}) where T
    return MOIU.operate!(+, T, MOIU.operate(-, T, convert(AffineDecisionFunction{T}, g)), f)
end
function MOIU.operate(op::Union{typeof(+), typeof(-)}, ::Type{T},
                      f::SingleDecision,
                      g::AffineDecisionFunction{T}) where T
    return MOIU.operate(op, T, convert(AffineDecisionFunction{T}, f), g)
end
function MOIU.operate(op::typeof(+), ::Type{T},
                      f::SingleDecision,
                      g::MOI.ScalarQuadraticFunction{T}) where T
    return MOIU.operate(op, T, convert(QuadraticDecisionFunction{T}, g), f)
end
function MOIU.operate(op::typeof(-), ::Type{T},
                      f::SingleDecision,
                      g::MOI.ScalarQuadraticFunction{T}) where T
    return MOIU.operate!(+, T, MOIU.operate(-, T, convert(QuadraticDecisionFunction{T}, g)), f)
end
function MOIU.operate(op::typeof(+), ::Type{T},
                      f::SingleDecision,
                      g::QuadraticDecisionFunction{T}) where T
    return MOIU.operate!(op, T, copy(g), f)
end
function MOIU.operate(op::typeof(-), ::Type{T},
                      f::SingleDecision,
                      g::QuadraticDecisionFunction{T}) where T
    return MOIU.operate!(+, T, operate(-, T, g), f)
end

# SingleVariable +- SingleDecision
function MOIU.operate(op::Union{typeof(+), typeof(-)}, ::Type{T},
                      f::MOI.SingleVariable,
                      g::SingleDecision) where T
    return MOIU.operate(op, T, g, f)
end

# Scalar Affine +/- ...
function MOIU.operate(op::Union{typeof(-)}, ::Type{T},
                      f::AffineDecisionFunction{T}) where T
    return AffineDecisionFunction(
        MOIU.operate(op, T, f.variable_part),
        MOIU.operate(op, T, f.decision_part))
end
function MOIU.operate(op::Union{typeof(+), typeof(-)}, ::Type{T},
                      f::AffineDecisionFunction{T},
                      g::ScalarAffineLike{T}) where T
    return MOIU.operate!(op, T, copy(f), g)
end
function MOIU.operate(op::Union{typeof(+), typeof(-)}, ::Type{T},
                      f::AffineDecisionFunction{T},
                      g::QuadraticDecisionFunction{T}) where T
    return QuadraticDecisionFunction(
            MOIU.operate(op, T, f.variable_part, g.variable_part),
            MOIU.operate(op, T, f.decision_part, g.decision_part),
            copy(g.cross_terms))
end
function MOIU.operate(op::Union{typeof(+), typeof(-)}, ::Type{T},
                      f::AffineDecisionFunction{T},
                      g::ScalarQuadraticLike{T}) where T
    return MOIU.operate(op, T, f, convert(QuadraticDecisionFunction{T}, g))
end

# Scalar Quadratic +/- ...
function MOIU.operate(op::Union{typeof(-)}, ::Type{T},
                      f::QuadraticDecisionFunction{T}) where T
    return QuadraticDecisionFunction(
            MOIU.operate(op, T, f.variable_part),
            MOIU.operate(op, T, f.decision_part),
            MOIU.operate(op, T, f.cross_terms))
end
function MOIU.operate(op::Union{typeof(+), typeof(-)}, ::Type{T},
                      f::QuadraticDecisionFunction{T},
                      g::ScalarQuadraticLike{T}) where T
    MOIU.operate!(op, T, copy(f), g)
end

# Base overloads
function Base.:+(arg::ScalarDecisionLike{T}, args::MixedScalarLike{T}...) where T
    return MOIU.operate(+, T, arg, args...)
end
function Base.:+(arg::MOIU.ScalarLike{T}, args::MixedScalarLike{T}...) where T
    return MOIU.operate(+, T, convert(AffineDecisionFunction{T}, arg), args...)
end
function Base.:+(α::T, arg::TypedScalarDecisionLike{T}, args::MixedScalarLike{T}...) where T
    return MOIU.operate(+, T, α, arg, args...)
end
function Base.:+(α::Number, f::SingleDecision)
    return MOIU.operate(+, typeof(α), α, f)
end
function Base.:+(f::TypedScalarDecisionLike{T}, α::T) where T
    return MOIU.operate(+, T, f, α)
end
function Base.:+(f::SingleDecision, α::Number)
    return MOIU.operate(+, typeof(α), f, α)
end
function Base.:-(arg::ScalarDecisionLike{T}, args::MixedScalarLike{T}...) where T
    return MOIU.operate(-, T, arg, args...)
end
function Base.:-(arg::MOIU.ScalarLike{T}, args::MixedScalarLike{T}...) where T
    return MOIU.operate(-, T, convert(AffineDecisionFunction{T}, arg), args...)
end
function Base.:-(f::TypedScalarDecisionLike{T}, α::T) where T
    return MOIU.operate(-, T, f, α)
end
function Base.:-(f::SingleDecision, α::Number)
    return MOIU.operate(-, typeof(α), f, α)
end
function Base.:-(α::T, f::TypedScalarDecisionLike{T}) where T
    return MOIU.operate(-, T, α, f)
end
function Base.:-(α::Number, f::SingleDecision)
    return MOIU.operate(-, typeof(α), α, f)
end

# VectorDecisionFunctions +/-
# ========================== #
function MOIU.promote_operation(::typeof(-), ::Type{T},
                                ::Type{<:VectorAffineDecisionLike{T}}) where T
    return VectorAffineDecisionFunction{T}
end

function MOIU.promote_operation(::Union{typeof(+), typeof(-)}, ::Type{T},
                                ::Type{<:VectorAffineDecisionLike{T}},
                                ::Type{<:VectorAffineLike{T}}) where T
    return VectorAffineDecisionFunction{T}
end
function MOIU.promote_operation(::Union{typeof(+), typeof(-)}, ::Type{T},
                                ::Type{<:MOIU.VectorAffineLike{T}},
                                ::Type{<:VectorAffineDecisionLike}) where T
    return VectorAffineDecisionFunction{T}
end

# Vector Decision +/- ...
function MOIU.operate!(op::Union{typeof(+), typeof(-)}, ::Type{T},
                       f::VectorOfDecisions,
                       g::VectorAffineDecisionLike{T}) where T
    return operate(op, T, f, g)
end
# VectorAffineDecision +/-! ...
function MOIU.operate_output_index!(
    op::Union{typeof(+), typeof(-)}, ::Type{T},
    output_index::Integer,
    f::VectorAffineDecisionFunction{T}, α::T) where T
    MOIU.operate_output_index!(op, T, output_index, f.variable_part, α)
    return f
end
function MOIU.operate!(op::Union{typeof(+), typeof(-)}, ::Type{T},
                       f::VectorAffineDecisionFunction{T},
                       g::Vector{T}) where T
    MOIU.operate!(op, T, f.variable_part, g)
    return f
end
function MOIU.operate_output_index!(
    op::Union{typeof(+), typeof(-)}, ::Type{T},
    output_index::Integer,
    f::VectorAffineDecisionFunction{T},
    g::MOI.SingleVariable) where T
    MOIU.operate_output_index!(op, T, output_index, f.variable_part, g)
    return f
end
function MOIU.operate_output_index!(
    op::Union{typeof(+), typeof(-)}, ::Type{T},
    output_index::Integer,
    f::VectorAffineDecisionFunction{T},
    g::SingleDecision) where T
    push!(f.decision_part.terms, MOI.VectorAffineTerm(
        output_index, MOI.ScalarAffineTerm(op(one(T)), g.decision)))
    return f
end
function MOIU.operate!(op::Union{typeof(+), typeof(-)}, ::Type{T},
                       f::VectorAffineDecisionFunction{T},
                       g::MOI.VectorOfVariables) where T
    MOIU.operate!(op, T, f.variable_part, g)
    return f
end
function MOIU.operate!(op::Union{typeof(+), typeof(-)}, ::Type{T},
                       f::VectorAffineDecisionFunction{T},
                       g::VectorOfDecisions) where T
    d = MOI.output_dimension(g)
    @assert MOI.output_dimension(f.decision_part) == d
    append!(f.decision_part.terms, MOI.VectorAffineTerm.(
        collect(1:d),
        MOI.ScalarAffineTerm.(op(one(T)), g.decisions)))
    return f
end
function MOIU.operate_output_index!(
    op::Union{typeof(+), typeof(-)}, ::Type{T},
    output_index::Integer,
    f::VectorAffineDecisionFunction{T},
    g::MOI.ScalarAffineFunction{T}) where T
    MOIU.operate_output_index!(op, T, output_index, f.variable_part, g)
    return f
end
function MOIU.operate_output_index!(
    op::Union{typeof(+), typeof(-)}, ::Type{T},
    output_index::Integer,
    f::VectorAffineDecisionFunction{T},
    g::AffineDecisionFunction{T}) where T
    MOIU.operate_output_index!(op, T, output_index, f.variable_part, g.variable_part)
    MOIU.operate_output_index!(op, T, output_index, f.decision_part, g.decision_part)
    return f
end
function MOIU.operate!(op::Union{typeof(+), typeof(-)}, ::Type{T},
                       f::VectorAffineDecisionFunction{T},
                       g::VectorAffineDecisionFunction{T}) where T
    MOIU.operate!(op, T, f.variable_part, g.variable_part)
    MOIU.operate!(op, T, f.decision_part, g.decision_part)
    return f
end

function MOIU.operate(op::typeof(-), ::Type{T}, f::VectorOfDecisions) where T
    d = MOI.output_dimension(f)
    return VectorAffineDecisionFunction{T}(
        MOI.VectorAffineFunction(MOI.VectorAffineTerm{T}[], zeros(T, d)),
        MOI.VectorAffineFunction(MOI.VectorAffineTerm.(
            collect(1:d),
            MOI.ScalarAffineTerm.(-one(T), f.decisions)),
                                 fill(zero(T),d)))
end

# VectorDecision number +/- ...
function MOIU.operate(op::typeof(+), ::Type{T}, α::Vector{T}, f::Union{VectorOfDecisions, VectorAffineDecisionFunction{T}}) where T
    return MOIU.operate(op, T, f, α)
end
function MOIU.operate(op::typeof(-), ::Type{T}, α::Vector{T}, f::Union{VectorOfDecisions, VectorAffineDecisionFunction{T}}) where T
    return MOIU.operate!(+, T, operate(-, T, f), α)
end

# Vector Decisions +/- ...
function MOIU.operate(op::Union{typeof(+), typeof(-)}, ::Type{T},
                      f::VectorOfDecisions, α::Vector{T}) where T
    d = MOI.output_dimension(f)
    @assert length(α) == d
    return VectorAffineDecisionFunction{T}(
        MOI.VectorAffineFunction(MOI.VectorAffineTerm{T}[], op.(α)),
        MOI.VectorAffineFunction(MOI.VectorAffineTerm.(
            collect(1:d),
            MOI.ScalarAffineTerm.(one(T), f.decisions)),
                                 zeros(T, d)))
end
function MOIU.operate(op::Union{typeof(+), typeof(-)}, ::Type{T},
                      f::VectorOfDecisions,
                      g::MOI.VectorOfVariables) where T
    d = MOI.output_dimension(f)
    @assert MOI.output_dimension(g) == d
    return VectorAffineDecisionFunction{T}(
        MOI.VectorAffineFunction(MOI.VectorAffineTerm.(
            collect(1:d),
            MOI.ScalarAffineTerm.(op(one(T)), g.variables)),
                                 zeros(T, d)),
        MOI.VectorAffineFunction(MOI.VectorAffineTerm.(
            collect(1:d),
            MOI.ScalarAffineTerm.(one(T), f.decisions)),
                                 zeros(T, d)))
end
function MOIU.operate(op::Union{typeof(+), typeof(-)}, ::Type{T},
                      f::VectorOfDecisions,
                      g::VectorOfDecisions) where T
    d = MOI.output_dimension(f)
    @assert MOI.output_dimension(g) == d
    return VectorAffineDecisionFunction{T}(
        MOI.VectorAffineFunction(MOI.VectorAffineTerm{T}[], zeros(T, d)),
        MOI.VectorAffineFunction(
            vcat(
                MOI.VectorAffineTerm.(
                    collect(1:d),
                    MOI.ScalarAffineTerm.(one(T), f.decisions)),
                MOI.VectorAffineTerm.(
                    collect(1:d),
                    MOI.ScalarAffineTerm.(op(one(T)), g.decisions))),
            zeros(T, d)))
end
function MOIU.operate(op::Union{typeof(+), typeof(-)}, ::Type{T},
                      f::VectorOfDecisions,
                      g::MOI.VectorAffineFunction{T}) where T
    d = MOI.output_dimension(f)
    @assert MOI.output_dimension(g) == d
    return VectorAffineDecisionFunction{T}(
        MOIU.operate(op, T, g),
        MOI.VectorAffineFunction(
            MOI.VectorAffineTerm.(
                collect(1:d),
                MOI.ScalarAffineTerm.(one(T), f.decisions)),
            zeros(T, d)))
end

# Vector Variables +- Vector Decisions
function MOIU.operate(op::Union{typeof(+), typeof(-)}, ::Type{T},
                      f::MOI.VectorOfVariables,
                      g::VectorOfDecisions) where T
    return MOIU.operate(op, T, convert(VectorAffineDecisionFunction{T}, f), g)
end

# Vector Decisions +- VectorAffineFunction
function MOIU.operate(op::typeof(+), ::Type{T},
                      f::VectorOfDecisions,
                      g::VectorAffineDecisionFunction{T}) where T
    MOIU.operate!(op, T, convert(VectorAffineDecisionFunction{T}, f), g)
end
function MOIU.operate(op::typeof(-), ::Type{T},
                      f::VectorOfDecisions,
                      g::VectorAffineDecisionFunction{T}) where T
    MOIU.operate!(op, T, convert(VectorAffineDecisionFunction{T}, f), g)
end

# Vector Affine +/- ...
function MOIU.operate(op::Union{typeof(-)}, ::Type{T},
                      f::VectorAffineDecisionFunction{T}) where T
    return VectorAffineDecisionFunction(
        MOIU.operate(op, T, f.variable_part),
        MOIU.operate(op, T, f.decision_part))
end
function MOIU.operate(op::Union{typeof(+), typeof(-)}, ::Type{T},
                      f::VectorAffineDecisionFunction{T},
                      g::VectorAffineLike{T}) where T
    return MOIU.operate!(op, T, copy(f), g)
end

function Base.:+(arg::VectorDecisionLike{T}, args::MixedVectorLike{T}...) where T
    return MOIU.operate(+, T, arg, args...)
end
function Base.:+(arg::MOIU.VectorLike{T}, args::MixedVectorLike{T}...) where T
    return MOIU.operate(+, T, convert(VectorAffineDecisionFunction{T}, arg), args...)
end
function Base.:+(α::Vector{T}, f::VectorDecisionLike{T}, g::MixedVectorLike{T}...) where T
    return MOIU.operate(+, T, α, f, g...)
end
function Base.:+(f::VectorDecisionLike{T}, α::Vector{T}) where T
    return MOIU.operate(+, T, f, α)
end
function Base.:-(arg::VectorDecisionLike{T}, args::MixedVectorLike{T}...) where T
    return MOIU.operate(-, T, arg, args...)
end
function Base.:-(arg::MOIU.VectorLike{T}, args::MixedVectorLike{T}...) where T
    return MOIU.operate(-, T, convert(VectorAffineDecisionFunction{T}, arg), args...)
end
function Base.:-(f::VectorDecisionLike{T}, α::Vector{T}) where T
    return MOIU.operate(-, T, f, α)
end
function Base.:-(α::Vector{T}, f::VectorDecisionLike{T}) where T
    return MOIU.operate(-, T, α, f)
end

# * #
# ========================== #
function MOIU.promote_operation(::typeof(*), ::Type{T}, ::Type{T},
                                ::Type{<:ScalarAffineDecisionLike{T}}) where T
    return AffineDecisionFunction{T}
end
function MOIU.promote_operation(::typeof(*), ::Type{T},
                                ::Type{<:ScalarAffineDecisionLike{T}},
                                ::Type{T}) where T
    return AffineDecisionFunction{T}
end
function MOIU.promote_operation(::typeof(*), ::Type{T}, ::Type{T},
                                ::Type{QuadraticDecisionFunction{T}}) where T
    return QuadraticDecisionFunction{T}
end
function MOIU.promote_operation(::typeof(*), ::Type{T},
                                ::Type{QuadraticDecisionFunction{T}},
                                ::Type{T}) where T
    return QuadraticDecisionFunction{T}
end
function MOIU.promote_operation(::typeof(*), ::Type{T},
                                ::Type{<:ScalarAffineDecisionLike{T}},
                                ::Type{<:ScalarAffineLike{T}}) where T
    return QuadraticDecisionFunction{T}
end
function MOIU.promote_operation(::typeof(*), ::Type{T},
                                ::Type{<:MOIU.ScalarAffineLike{T}},
                                ::Type{<:ScalarAffineDecisionLike{T}}) where T
    return QuadraticDecisionFunction{T}
end

function MOIU.operate!(::typeof(*), ::Type{T}, f::SingleDecision, α::T) where T
    return MOIU.operate(*, T, α, f)
end
function MOIU.operate(::typeof(*), ::Type{T}, α::T, f::SingleDecision) where T
    return AffineDecisionFunction{T}(convert(MOIU.ScalarAffineLike{T}, zero(T)),
                                     MOI.ScalarAffineFunction{T}([MOI.ScalarAffineTerm(α, f.decision)], zero(T)))
end
function MOIU.operate(::typeof(*), ::Type{T}, α::T, f::VectorOfDecisions) where T
    d = MOI.output_dimension(f)
    return VectorAffineDecisionFunction{T}(
        MOI.VectorAffineFunction(MOI.VectorAffineTerm{T}[], zeros(T, d)),
        MOI.VectorAffineFunction(
            [MOI.VectorAffineTerm(i, MOI.ScalarAffineTerm(α, f.decisions[i]))
             for i in eachindex(f.decisions)], zeros(T, MOI.output_dimension(f))))
end
function MOIU.operate(::typeof(*), ::Type{T}, f::Union{SingleDecision, VectorOfDecisions}, α::T) where T
    return MOIU.operate(*, T, α, f)
end

function MOIU.operate!(::typeof(*), ::Type{T},
                       f::AffineDecisionFunction{T}, α::T) where T<:Number
    MOIU.operate!(*, T, f.variable_part, α)
    MOIU.operate!(*, T, f.decision_part, α)
    return f
end
function MOIU.operate!(::typeof(*), ::Type{T},
                       f::QuadraticDecisionFunction{T}, α::T) where T<:Number
    MOIU.operate!(*, T, f.variable_part, α)
    MOIU.operate!(*, T, f.decision_part, α)
    MOIU.operate!(*, T, f.cross_terms, α)
    return f
end

function MOIU.operate!(::typeof(*), ::Type{T},
                       f::VectorAffineDecisionFunction{T}, α::T) where T
    MOIU.operate!(*, T, f.variable_part, α)
    MOIU.operate!(*, T, f.decision_part, α)
    return f
end

function MOIU.operate(::typeof(*), ::Type{T}, α::T, f::TypedDecisionLike{T}) where T
    return MOIU.operate!(*, T, copy(f), α)
end

function MOIU.operate(::typeof(*), ::Type{T}, f::SingleDecision,
                      g::MOI.SingleVariable) where T
    return QuadraticDecisionFunction(
        convert(MOI.ScalarQuadraticFunction{T}, zero(T)),
        convert(MOI.ScalarQuadraticFunction{T}, zero(T)),
        MOI.ScalarQuadraticFunction{T}(
            MOI.ScalarAffineTerm{T}[],
            [MOI.ScalarQuadraticTerm(one(T), f.decision, g.variable)],
            zero(T)))
end
function MOIU.operate(::typeof(*), ::Type{T}, f::SingleDecision,
                      g::SingleDecision) where T
    return QuadraticDecisionFunction(
        convert(MOI.ScalarQuadraticFunction{T}, zero(T)),
        MOI.ScalarQuadraticFunction(
            MOI.ScalarAffineTerm{T}[],
            [MOI.ScalarQuadraticTerm(f.decision == g.decision ? 2one(T) : one(T),
                                     f.decision, g.decision)],
            zero(T)),
        convert(MOI.ScalarQuadraticFunction{T}, zero(T)))
end

function MOIU.operate(::typeof(*), ::Type{T}, f::MOI.SingleVariable, g::SingleDecision) where T
    return MOIU.operate(*, T, g, f)
end

function MOIU.operate(::typeof(*), ::Type{T}, f::AffineDecisionFunction{T},
                      g::AffineDecisionFunction{T}) where T
    # Variable part
    variable_part = MOIU.operate(*, T, f.variable_part, g.variable_part)
    # Decision part
    decision_part = MOIU.operate(*, T, f.decision_part, g.decision_part)
    if !iszero(f.variable_part.constant)
        for term in g.decision_part.terms
            new_term = MOIU.operate_term(*, f.variable_part.constant, term)
            add_term!(decision_part.affine_terms, new_term)
        end
    end
    if !iszero(g.variable_part.constant)
        for term in f.decision_part.terms
            new_term = MOIU.operate_term(*, g.variable_part.constant, term)
            add_term!(decision_part.affine_terms, new_term)
        end
    end
    # Cross terms
    cross_terms = convert(MOI.ScalarQuadraticFunction{T}, zero(T))
    for var_term in f.variable_part.terms
        for dvar_term in g.decision_part.terms
            new_term = MOIU.operate_term(*, dvar_term, var_term)
            add_term!(cross_terms.quadratic_terms, new_term)
        end
    end
    for var_term in g.variable_part.terms
        for dvar_term in f.decision_part.terms
            new_term = MOIU.operate_term(*, dvar_term, var_term)
            add_term!(cross_terms.quadratic_terms, new_term)
        end
    end
    return QuadraticDecisionFunction(
        variable_part,
        decision_part,
        cross_terms)
end
function MOIU.operate(::typeof(*), ::Type{T}, f::AffineDecisionFunction{T},
                      g::ScalarAffineLike{T}) where T
    return MOIU.operate(*, T, f, convert(AffineDecisionFunction{T}, g))
end
function MOIU.operate(::typeof(*), ::Type{T}, f::ScalarAffineLike{T},
                      g::AffineDecisionFunction{T}) where T
    return MOIU.operate(*, T, g, convert(AffineDecisionFunction{T}, f))
end
function MOIU.operate(::typeof(*), ::Type{T}, α::T,
                      g::AffineDecisionFunction{T}) where T
    return MOIU.operate!(*, T, copy(g), α)
end

MOIU.is_coefficient_type(::Type{<:Union{SingleDecision, VectorOfDecisions}}, ::Type) = true
MOIU.is_coefficient_type(::Type{<:TypedDecisionLike{T}}, ::Type{T}) where T = true
MOIU.is_coefficient_type(::Type{<:TypedDecisionLike}, ::Type) = false

function Base.:*(f::ScalarDecisionLike{T}, g::MixedScalarLike{T}, args::MixedScalarLike{T}...) where T
    return MOIU.operate(*, T, f, g, args...)
end
function Base.:*(f::MOIU.ScalarLike{T}, g::MixedScalarLike{T}, args::MixedScalarLike{T}...) where T
    return MOIU.operate(*, T, convert(AffineDecisionFunction{T}, f), g, args...)
end
function Base.:*(α::T, g::TypedDecisionLike{T}) where T
    return MOIU.operate_coefficients(β -> α * β, g)
end
function Base.:*(α::Number, g::TypedDecisionLike)
    return MOIU.operate_coefficients(β -> α * β, g)
end
function Base.:*(α::T, g::TypedDecisionLike{T}) where T <: Number
    return MOIU.operate_coefficients(β -> α * β, g)
end
function Base.:*(f::Number, g::Union{SingleDecision, VectorOfDecisions})
    return MOIU.operate(*, typeof(f), f, g)
end
function Base.:*(f::TypedDecisionLike{T}, α::T) where T
    return MOIU.operate_coefficients(β -> α * β, f)
end
function Base.:*(f::TypedDecisionLike, α::Number)
    return MOIU.operate_coefficients(β -> α * β, f)
end
function Base.:*(f::TypedDecisionLike{T}, α::T) where T <: Number
    return MOIU.operate_coefficients(β -> α * β, f)
end
function Base.:*(f::Union{SingleDecision, VectorOfDecisions}, g::Number)
    return MOIU.operate(*, typeof(g), f, g)
end
function Base.:*(f::TypedDecisionLike{T}, g::Bool) where T
    if g
        return MA.copy_if_mutable(f)
    else
        return zero(typeof(f))
    end
end

function Base.:^(func::AffineDecisionFunction{T}, p::Integer) where T
    if iszero(p)
        return one(QuadraticDecisionFunction{T})
    elseif isone(p)
        return convert(QuadraticDecisionFunction{T}, func)
    elseif p == 2
        return func * func
    else
        throw(ArgumentError("Cannot take $(typeof(func)) to the power $p."))
    end
end
function Base.:^(func::QuadraticDecisionFunction{T}, p::Integer) where T
    if iszero(p)
        return one(QuadraticDecisionFunction{T})
    elseif isone(p)
        return MA.mutable_copy(func)
    else
        throw(ArgumentError("Cannot take $(typeof(func)) to the power $p."))
    end
end

function LinearAlgebra.dot(f::ScalarDecisionLike, g::ScalarDecisionLike)
    return f * g
end
function LinearAlgebra.dot(α::T, func::TypedDecisionLike{T}) where T
    return α * func
end
function LinearAlgebra.dot(func::TypedDecisionLike{T}, α::T) where T
    return func * α
end
function LinearAlgebra.dot(f::VectorAffineDecisionFunction{T}, g::VectorAffineDecisionFunction{T}) where T
    result = zero(QuadraticDecisionFunction{T})
    for (lhs,rhs) in zip(MOIU.scalarize(f), MOIU.scalarize(g))
        term = MOIU.operate(*, T, lhs, rhs)
        MOIU.operate!(+, T, result, term)
    end
    return result
end

LinearAlgebra.adjoint(f::ScalarDecisionLike) = f
LinearAlgebra.transpose(f::ScalarDecisionLike) = f
LinearAlgebra.symmetric_type(::Type{F}) where {F <: ScalarDecisionLike} = F
LinearAlgebra.symmetric(f::ScalarDecisionLike, ::Symbol) = f

# / #
# ========================== #
function MOIU.promote_operation(::typeof(/), ::Type{T},
                                ::Type{<:ScalarAffineDecisionLike{T}},
                                ::Type{T}) where T
    AffineDecisionFunction{T}
end
function MOIU.promote_operation(::typeof(/), ::Type{T},
                                ::Type{QuadraticDecisionFunction{T}},
                                ::Type{T}) where T
    QuadraticDecisionFunction{T}
end

function MOIU.operate!(::typeof(/), ::Type{T}, f::SingleDecision,
                       α::T) where T
    return MOIU.operate(/, T, f, α)
end
function MOIU.operate(::typeof(/), ::Type{T},
                      α::T,
                      f::Union{SingleDecision, VectorOfDecisions}) where T
    return MOIU.operate(*, T, inv(α), f)
end
function MOIU.operate(::typeof(/), ::Type{T},
                      f::Union{SingleDecision, VectorOfDecisions},
                      α::T) where T
    return MOIU.operate(*, T, inv(α), f)
end

function MOIU.operate!(::typeof(/), ::Type{T},
                       f::Union{AffineDecisionFunction{T}, VectorAffineDecisionFunction{T}},
                       α::T) where T
    MOIU.operate!(/, T, f.variable_part, α)
    MOIU.operate!(/, T, f.decision_part, α)
    return f
end

function MOIU.operate!(::typeof(/), ::Type{T},
                       f::QuadraticDecisionFunction{T},
                       α::T) where T
    MOIU.operate!(/, T, f.variable_part, α)
    MOIU.operate!(/, T, f.decision_part, α)
    MOIU.operate!(/, T, f.cross_terms, α)
    return f
end

function MOIU.operate(::typeof(/), ::Type{T}, f::TypedDecisionLike{T}, α::T) where T
    return MOIU.operate!(/, T, copy(f), α)
end

function Base.:/(f::TypedDecisionLike{T}, g::T) where T
    return MOIU.operate(/, T, f, g)
end
function Base.:/(f::Union{SingleDecision, VectorOfDecisions}, g::Number)
    return MOIU.operate(/, typeof(g), f, g)
end

# vcat #
# ========================== #
function MOIU.fill_variables(decisions::Vector{MOI.VariableIndex}, offset::Int,
                             output_offset::Int, f::SingleDecision)
    decisions[offset + 1] = f.decision
end

function MOIU.fill_variables(decisions::Vector{MOI.VariableIndex}, offset::Int,
                             output_offset::Int, f::VectorOfDecisions)
    decisions[offset .+ (1:length(f.decisions))] .= f.decisions
end

function MOIU.promote_operation(::typeof(vcat), ::Type{T},
                                ::Type{<:Union{SingleDecision,
                                               VectorOfDecisions}}...) where T
    return VectorOfDecisions
end
function MOIU.operate(::typeof(vcat), ::Type{T},
                      funcs::Union{SingleDecision,
                                   VectorOfDecisions}...) where T
    out_dim = sum(func -> MOIU.output_dim(T, func), funcs)
    decisions = Vector{MOI.VariableIndex}(undef, out_dim)
    MOIU.fill_vector(decisions, T, 0, 0, MOIU.fill_variables, MOIU.output_dim, funcs...)
    return VectorOfDecisions(decisions)
end

function MOIU.promote_operation(::typeof(vcat), ::Type{T},
                                ::Type{<:AffineDecisionLike{T}},
                                ::Type{<:AffineLike{T}}...) where T
    return VectorAffineDecisionFunction{T}
end

function MOIU.promote_operation(::typeof(vcat), ::Type{T},
                                ::Type{QuadraticDecisionFunction{T}}) where T
    return QuadraticDecisionFunction{T}
end

function MOIU.operate(::typeof(vcat), ::Type{T},
                      funcs::VectorAffineDecisionFunction{T}...) where T
    # Variable part
    nvariable_terms = sum(f -> MOIU.number_of_affine_terms(T, f.variable_part), funcs)
    variable_out_dim = sum(f -> MOIU.output_dim(T, f.variable_part), funcs)
    variable_terms = Vector{MOI.VectorAffineTerm{T}}(undef, nvariable_terms)
    variable_constants = zeros(T, variable_out_dim)
    MOIU.fill_vector(variable_terms, T, 0, 0,
                     MOIU.fill_terms, MOIU.number_of_affine_terms, [f.variable_part for f in funcs]...)
    MOIU.fill_vector(variable_constants, T, 0, 0,
                     MOIU.fill_constant, MOIU.output_dim, [f.variable_part for f in funcs]...)
    # Decision part
    ndecision_terms = sum(f -> MOIU.number_of_affine_terms(T, f.decision_part), funcs)
    decision_out_dim = sum(f -> MOIU.output_dim(T, f.decision_part), funcs)
    decision_terms = Vector{MOI.VectorAffineTerm{T}}(undef, ndecision_terms)
    decision_constants = zeros(T, decision_out_dim)
    MOIU.fill_vector(decision_terms, T, 0, 0,
                     MOIU.fill_terms, MOIU.number_of_affine_terms, [f.decision_part for f in funcs]...)
    MOIU.fill_vector(decision_constants, T, 0, 0,
                     MOIU.fill_constant, MOIU.output_dim, [f.decision_part for f in funcs]...)
    # Pad output
    out_dim = max(variable_out_dim, decision_out_dim)
    append!(variable_constants, zeros(T, out_dim - length(variable_constants)))
    append!(decision_constants, zeros(T, out_dim - length(decision_constants)))
    return VectorAffineDecisionFunction{T}(MOIU.VAF(variable_terms, variable_constants),
                                           MOIU.VAF(decision_terms, decision_constants))
end

# First or second argument must be decision like to avoid type piracy
function MOIU.operate(::typeof(vcat), ::Type{T},
                      f::AffineDecisionLike{T},
                      funcs::AffineLike{T}...) where T
    return MOIU.operate(vcat,
                        T,
                        convert(VectorAffineDecisionFunction{T}, f),
                        [convert(VectorAffineDecisionFunction{T}, f) for f in funcs]...)
end
function MOIU.operate(::typeof(vcat), ::Type{T},
                      f::Union{MOIU.ScalarAffineLike{T}, MOIU.VectorAffineLike{T}},
                      g::AffineDecisionLike{T},
                      funcs::AffineLike{T}...) where T
    return MOIU.operate(vcat,
                        T,
                        convert(VectorAffineDecisionFunction{T}, f),
                        convert(VectorAffineDecisionFunction{T}, g),
                        [convert(VectorAffineDecisionFunction{T}, f) for f in funcs]...)
end

Base.promote_rule(::Type{F}, ::Type{T}) where {T, F<:Union{AffineDecisionFunction{T}, VectorAffineDecisionFunction{T}}} = F


function build_robot!(vis::Visualizer, model::Biped;
    r=0.035, r_contact=r * 8.0 / 7.0, color_opacity=1.0)

    r = convert(Float32, r)
    r_contact = convert(Float32, r_contact)

    body_mat = MeshPhongMaterial(color = RGBA(0.0, 0.0, 0.0, color_opacity))
    contact_mat = MeshPhongMaterial(color = RGBA(1.0, 165.0 / 255.0, 0.0, color_opacity))

    default_background!(vis)

    torso = GeometryBasics.Cylinder(Point3f0(0.0), Point3f0(0.0, 0.0, model.l_torso), r)
    setobject!(vis[:robot]["torso"], torso, body_mat)

    thigh_1 = GeometryBasics.Cylinder(Point3f0(0.0), Point3f0(0.0, 0.0, model.l_thigh1), r)
    setobject!(vis[:robot]["thigh1"], thigh_1, body_mat)

    calf_1 = GeometryBasics.Cylinder(Point3f0(0.0), Point3f0(0.0, 0.0, model.l_calf1), r)
    setobject!(vis[:robot]["calf1"], calf_1, body_mat)

    foot_1 = GeometryBasics.Cylinder(Point3f0(0.0), Point3f0(0.0, 0.0, model.lt_foot1 + model.lh_foot1), r)
    setobject!(vis[:robot]["foot1"], foot_1, body_mat)

    thigh_2 = GeometryBasics.Cylinder(Point3f0(0.0), Point3f0(0.0, 0.0, model.l_thigh2), r)
    setobject!(vis[:robot]["thigh2"], thigh_2, body_mat)

    calf_2 = GeometryBasics.Cylinder(Point3f0(0.0), Point3f0(0.0, 0.0, model.l_calf2), r)
    setobject!(vis[:robot]["calf2"], calf_2, body_mat)

    foot_2 = GeometryBasics.Cylinder(Point3f0(0.0), Point3f0(0.0, 0.0, model.lt_foot2 + model.lh_foot2), r)
    setobject!(vis[:robot]["foot2"], foot_2, body_mat)

    setobject!(vis[:robot]["heel1"], GeometryBasics.Sphere(Point3f0(0.0), r_contact), contact_mat)
    setobject!(vis[:robot]["heel2"], GeometryBasics.Sphere(Point3f0(0.0), r_contact), contact_mat)
    setobject!(vis[:robot]["toe1"], GeometryBasics.Sphere(Point3f0(0.0), r_contact), contact_mat)
    setobject!(vis[:robot]["toe2"], GeometryBasics.Sphere(Point3f0(0.0), r_contact), contact_mat)
    setobject!(vis[:robot]["knee1"], GeometryBasics.Sphere(Point3f0(0.0), r), body_mat)
    setobject!(vis[:robot]["knee2"], GeometryBasics.Sphere(Point3f0(0.0), r), body_mat)
    setobject!(vis[:robot]["hip"], GeometryBasics.Sphere(Point3f0(0.0), r), body_mat)
    setobject!(vis[:robot]["torso_top"], GeometryBasics.Sphere(Point3f0(0.0), r), body_mat)

    return true
end

function set_robot!(vis::Visualizer, model::Biped, q;
   r=0.035, r_contact=r * 8.0 / 7.0)

    p_shift = [0.0; 0.0; r_contact]

    k_hip = kinematics_body(model, q, mode=:hip)
    p_hip = [k_hip[1]; 0.0; k_hip[2]] + p_shift

    k_torso = kinematics_body(model, q, mode=:ee)
    p_torso = [k_torso[1], 0.0, k_torso[2]] + p_shift

    k_thigh_1 = kinematics_thigh(model, q, leg=:leg1, mode=:ee)
    p_thigh_1 = [k_thigh_1[1], 0.0, k_thigh_1[2]] + p_shift

    k_calf_1 = kinematics_calf(model, q, leg=:leg1, mode=:ee)
    p_calf_1 = [k_calf_1[1], 0.0, k_calf_1[2]] + p_shift

    k_toe_1 = kinematics_foot(model, q, leg=:leg1, mode=:toe)
    p_toe_1 = [k_toe_1[1], 0.0, k_toe_1[2]] + p_shift

    k_heel_1 = kinematics_foot(model, q, leg=:leg1, mode=:heel)
    p_heel_1 = [k_heel_1[1], 0.0, k_heel_1[2]] + p_shift

    k_thigh_2 = kinematics_thigh(model, q, leg=:leg2, mode=:ee)
    p_thigh_2 = [k_thigh_2[1], 0.0, k_thigh_2[2]] + p_shift

    k_calf_2 = kinematics_calf(model, q, leg=:leg2, mode=:ee)
    p_calf_2 = [k_calf_2[1], 0.0, k_calf_2[2]] + p_shift

    k_toe_2 = kinematics_foot(model, q, leg=:leg2, mode=:toe)
    p_toe_2 = [k_toe_2[1], 0.0, k_toe_2[2]] + p_shift

    k_heel_2 = kinematics_foot(model, q, leg=:leg2, mode=:heel)
    p_heel_2 = [k_heel_2[1], 0.0, k_heel_2[2]] + p_shift

    settransform!(vis[:robot]["thigh1"], cable_transform(p_hip, p_thigh_1))
    settransform!(vis[:robot]["calf1"], cable_transform(p_thigh_1, p_calf_1))
    settransform!(vis[:robot]["foot1"], cable_transform(p_toe_1, p_heel_1))

    settransform!(vis[:robot]["thigh2"], cable_transform(p_hip, p_thigh_2))
    settransform!(vis[:robot]["calf2"], cable_transform(p_thigh_2, p_calf_2))
    settransform!(vis[:robot]["foot2"], cable_transform(p_toe_2, p_heel_2))

    settransform!(vis[:robot]["torso"], cable_transform(p_hip, p_torso))
    settransform!(vis[:robot]["heel1"], Translation(p_heel_1))
    settransform!(vis[:robot]["heel2"], Translation(p_heel_2))
    settransform!(vis[:robot]["toe1"], Translation(p_toe_1))
    settransform!(vis[:robot]["toe2"], Translation(p_toe_2))
    settransform!(vis[:robot]["knee1"], Translation(p_thigh_1))
    settransform!(vis[:robot]["knee2"], Translation(p_thigh_2))
    settransform!(vis[:robot]["hip"], Translation(p_hip))
    settransform!(vis[:robot]["torso_top"], Translation(p_torso))

    return true
end

function visualize!(vis, model::Biped, q;
	Δt=0.1, r=0.04, r_contact=0.04, color_opacity=1.0, fixed_camera=true)

	build_robot!(vis, model, r=r, r_contact=r_contact, color_opacity=color_opacity)

	anim = MeshCat.Animation(convert(Int, floor(1.0 / Δt)))

	for (t, qt) in enumerate(q)
		MeshCat.atframe(anim, t) do
			set_robot!(vis, model, qt)
		end
	end

	MeshCat.setanimation!(vis, anim)

    if fixed_camera
        settransform!(vis["/Cameras/default"],
            compose(Translation(0.0, -50.0, -1.0),LinearMap(RotZ(-pi / 2.0))))
        setprop!(vis["/Cameras/default/rotated/<object>"], "zoom", 25)
    end

    return true
end


"""
    plan_prioritized_paths!(env::CoordinatedMAPFEnv, agent_ordering::Vector{Int64}, initial_states::Vector{MS}, cnr::Type{CNR}, pr::Type{PR})

Run the Prioritized Planning algorithm. Given an ordering over agents, plan a path for each of them
one-by-one and impose constraints derived from the path on subsequent agents.
"""
function plan_prioritized_paths!(env::CoordinatedMAPFEnv, agent_ordering::Vector{Int64}, initial_states::Vector{MS}, cnr::Type{CNR}, pr::Type{PR}) where {MS <: MAPFState, CNR <: MAPFConstraints, PR <: PlanResult}

    running_constraint = MultiAgentPathFinding.get_empty_constraint(cnr)
    paths = pr[]

    # Iterate over agent ordering
    for id in agent_ordering

        # Run low level search for each agent
        id_path = low_level_pp_search!(env, id, initial_states[id], running_constraint)

        # Update constraints
        add_constraint_from_pp_search!(env, running_constraint, id, id_path)

        # Add to path set
        push!(paths, id_path)

    end # id in agent_ordering
    return paths
end

## Just implement the various functions for aerial and ground here
function low_level_pp_search!(env::CoordinatedMAPFEnv, ground_agent_id::Int64, s::GroundMAPFState, constraint::GroundMAPFConstraint)

    env.ground_mapf_graph = SimpleVListGraph{GroundMAPFState}()
    empty!(env.unique_gmapf_states)

    Graphs.add_vertex!(env.ground_mapf_graph, s)
    env.unique_gmapf_states[s] = GMAPFStateInfo(1, 0.0, Set{Int64}())

    road_vtx_goal = env.ground_task_list[ground_agent_id].dest
    admissible_heuristic(u) = ground_mapf_heuristic(env, road_vtx_goal, u)
    edge_wt_fn(u, v) = get_ground_mapf_edge_weight(env, u, v)

    vis = GroundMAPFGoalVisitor(env, road_vtx_goal, constraint.avoid_edge_copy_set)

    start_idx = 1
    env.next_gmapfg_goal_idx = 0

    a_star_states = a_star_implicit_shortest_path!(env.ground_mapf_graph, edge_wt_fn, start_idx, vis, admissible_heuristic)

    @assert haskey(a_star_states.parent_indices, env.next_gmapfg_goal_idx)

    states = Tuple{GroundMAPFState,Float64}[]
    actions = Tuple{GroundMAPFAction,Float64}[]

    sp_cost = a_star_states.dists[env.next_gmapfg_goal_idx]

    sp_idxs = shortest_path_indices(a_star_states.parent_indices, env.ground_mapf_graph, start_idx, env.next_gmapfg_goal_idx)

    push!(states, (s, 0.0))

    for i = 2:length(sp_idxs)

        u_idx, v_idx =  (sp_idxs[i-1], sp_idxs[i])

        new_state = deepcopy(env.ground_mapf_graph.vertices[v_idx])
        push!(states, (new_state, a_star_states.dists[v_idx]))

        push!(actions, (get_mapf_ground_action(env, env.ground_mapf_graph.vertices[u_idx], env.ground_mapf_graph.vertices[v_idx]), a_star_states.dists[v_idx] - a_star_states.dists[u_idx]))
    end

    return PlanResult{GroundMAPFState,GroundMAPFAction,Float64}(states, actions, sp_cost, sp_cost)
end

function add_constraint_from_pp_search!(env::CoordinatedMAPFEnv, constraint::GroundMAPFConstraint, ground_id::Int64, id_path::PlanResult)

    for (id_action, _) in id_path.actions

        # Just add the aerial IDs for the edge
        if !isempty(id_action.edge_aerial_ids)
            if ~haskey(constraint.avoid_edge_copy_set, id_action.edge_id)
                constraint.avoid_edge_copy_set[id_action.edge_id] = Set{Int64}()
            end
            union!(constraint.avoid_edge_copy_set[id_action.edge_id], id_action.edge_aerial_ids)
        end
    end
end

function low_level_pp_search!(env::CoordinatedMAPFEnv, aerial_agent_id::Int64, s::AerialMAPFState, constraint::GroundTransitConstraint)

    env.aerial_mapf_graph = SimpleVListGraph{AerialMAPFState}()
    Graphs.add_vertex!(env.aerial_mapf_graph, s)

    env.gtg_idx_gval =  Dict{Int64,Float64}()

    edge_wt_fn(u, v) = amapfg_edge_wt_fn(env, u, v)
    admissible_heuristic(u) = direct_flight_time_heuristic(env, u)

    # Set start and goal
    start_idx = 1   # Always one since graph reinitialized
    env.next_gtg_goal_idx = env.gtg_drone_start_goal_idxs[aerial_agent_id][2]
    env.next_amapfg_goal_idx = 0

    vis = GroundTransitGoalVisitor(env, constraint.avoid_gid_vertex_set)

    a_star_states = a_star_implicit_shortest_path!(env.aerial_mapf_graph,
                                                    edge_wt_fn,
                                                    start_idx,
                                                    vis,
                                                    admissible_heuristic
                                                    )

    # Get solution from a_star_eps_states
    # NOTE: It should always have a solution because drone can just fly
    @assert haskey(a_star_states.parent_indices, env.next_amapfg_goal_idx)

    # Get path cost
    states = Tuple{AerialMAPFState,Float64}[]
    actions = Tuple{GroundTransitAction, Float64}[]

    sp_cost = a_star_states.dists[env.next_amapfg_goal_idx]

    # Get path
    sp_idxs = shortest_path_indices(a_star_states.parent_indices, env.aerial_mapf_graph, start_idx, env.next_amapfg_goal_idx)

    push!(states, (s, 0.0))

    # Traverse sp_idxs and set up states and actions
    for i = 2:length(sp_idxs)

        u, v =  (sp_idxs[i-1], sp_idxs[i])

        # NOTE: Create new state because we are going to delete graph later
        new_state = deepcopy(env.aerial_mapf_graph.vertices[v])
        push!(states, (new_state, a_star_states.dists[v]))

        # All derived info but can be useful
        push!(actions, (get_mapf_action(env, u, v), a_star_states.dists[v] - a_star_states.dists[u]))
    end

    return PlanResult{AerialMAPFState,GroundTransitAction,Float64}(states, actions, sp_cost, sp_cost)
end

function add_constraint_from_pp_search!(env::CoordinatedMAPFEnv, constraint::GroundTransitConstraint, aerial_id::Int64, id_path::PlanResult)

    for ((si, _), (sip, _)) in zip(id_path.states[1:end-1], id_path.states[2:end])
        si_gtg = env.ground_transit_graph.vertices[si.ground_transit_idx]
        sip_gtg = env.ground_transit_graph.vertices[sip.ground_transit_idx]
        if si_gtg.ground_agent_id !=0 && si_gtg.ground_agent_id == sip_gtg.ground_agent_id
            if ~haskey(env.gtg_idx_to_aerial_ids, si_gtg.idx)
                env.gtg_idx_to_aerial_ids[si_gtg.idx] = Int64[]
            end
            push!(env.gtg_idx_to_aerial_ids[si_gtg.idx], aerial_id)
        end
    end

    for (gtgidx, aerial_ids) in env.gtg_idx_to_aerial_ids
        if length(aerial_ids) > env.car_capacity
            gid = env.ground_transit_graph.vertices[gtgidx].ground_agent_id
            union!(constraint.avoid_gid_vertex_set, gid)
        end
    end

end

struct GroundTransitVertexGoalVisitor <: Graphs.AbstractDijkstraVisitor
    env::CoordinatedMAPFEnv
    avoid_gtg_vertex_set::Set{Int64}
end

function Graphs.include_vertex!(vis::GroundTransitVertexGoalVisitor, u::AerialMAPFState, v::AerialMAPFState, d::Float64, nbrs::Vector{Int64})

    env = vis.env
    avoid_gtg_vertex_set = vis.avoid_gtg_vertex_set
    gtg_weights = env.ground_transit_weights

    # When you expand goal, return
    if v.ground_transit_idx == env.next_gtg_goal_idx
        env.next_amapfg_goal_idx = v.idx
        return false
    end

    # Now lookup where drone is on the GTG
    ugtg = env.ground_transit_graph.vertices[u.ground_transit_idx]
    vgtg = env.ground_transit_graph.vertices[v.ground_transit_idx]
    idx_ctr = Graphs.num_vertices(env.aerial_mapf_graph)

    # Case 1: Drone start
    if vgtg.ground_agent_id == 0

        ddiff = gtg_weights[(v.ground_transit_idx, env.next_gtg_goal_idx)]
        tdiff = ddiff / AvgSpeed

        idx_ctr += 1
        new_goal_vert = AerialMAPFState(idx=idx_ctr, ground_transit_idx=env.next_gtg_goal_idx, timeval=v.timeval+tdiff, distval=v.distval+ddiff)

        # Add to graph and add neighbor
        Graphs.add_vertex!(env.aerial_mapf_graph, new_goal_vert)
        push!(nbrs, idx_ctr)

        env.gtg_idx_gval[env.next_gtg_goal_idx] = env.alpha_weight_distance*new_goal_vert.distval + (1.0 - env.alpha_weight_distance)*new_goal_vert.timeval

        # Now add other car connections while accounting for time
        # First get non-dominated gtg indices and then add aerial mapf state versions of them
        # For timeval use the max of gtg time and drone timeval
        for (_, (gstart, gend)) in enumerate(env.gtg_ground_path_start_ends)
            gts_non_dom = get_non_dominated_transit_points(env, gstart, gend, avoid_gtg_vertex_set, v, false)

            # Add AerialMAPFState versions of them
            for gts_tup in gts_non_dom

                idx_ctr += 1
                new_vert = AerialMAPFState(idx=idx_ctr, ground_transit_idx=gts_tup.idx, timeval=gts_tup.timeval, distval=gts_tup.distval)

                Graphs.add_vertex!(env.aerial_mapf_graph, new_vert)
                push!(nbrs, idx_ctr)

                env.gtg_idx_gval[gts_tup.idx] = env.alpha_weight_distance*new_vert.distval + (1.0 - env.alpha_weight_distance)*new_vert.timeval
            end
        end

    else
        # Either first transfer to new ground point OR along the way
        if vgtg.ground_agent_id != ugtg.ground_agent_id

            new_considered_cars = union(v.considered_cars, Set{Int64}([vgtg.ground_agent_id]))

            next_gtg = env.ground_transit_graph.vertices[vgtg.idx + 1]

            idx_ctr += 1
            # IMP NOTE: Timeval may be greater than vgtg.timeval (probably will be)
            # So the new timeval should be v.timeval + car_time_diff (next_gtg.timeval - vgtg.timeval)
            tval = v.timeval + (next_gtg.timeval - vgtg.timeval)
            new_vert = AerialMAPFState(idx=idx_ctr, ground_transit_idx=next_gtg.idx, timeval=tval, distval=v.distval, considered_cars=new_considered_cars)

            Graphs.add_vertex!(env.aerial_mapf_graph, new_vert)
            push!(nbrs, idx_ctr)

        else
            # Ongoing car route vertex. First try to add goal as usual
            ddiff = gtg_weights[(v.ground_transit_idx, env.next_gtg_goal_idx)]
            tdiff = ddiff / AvgSpeed
            goal_gval = env.alpha_weight_distance*(v.distval+ddiff) + (1.0 - env.alpha_weight_distance)*(v.timeval + tdiff)

            if goal_gval < env.gtg_idx_gval[env.next_gtg_goal_idx]

                idx_ctr += 1
                new_goal_vert = AerialMAPFState(idx=idx_ctr, ground_transit_idx=env.next_gtg_goal_idx, timeval=v.timeval+tdiff, distval=v.distval+ddiff, considered_cars=v.considered_cars)

                # Add to graph and add neighbor
                Graphs.add_vertex!(env.aerial_mapf_graph, new_goal_vert)
                push!(nbrs, idx_ctr)

                env.gtg_idx_gval[env.next_gtg_goal_idx] = goal_gval
            end

            # Then add next along car route IF not last
            if vgtg.idx < env.gtg_ground_path_start_ends[vgtg.ground_agent_id][2]
                next_gtg = env.ground_transit_graph.vertices[vgtg.idx + 1]
                # @assert next_gtg.ground_agent_id == vgtg.ground_agent_id && next_gtg.vertex_along_path == vgtg.vertex_along_path+1

                idx_ctr += 1
                # IMP NOTE: Timeval may be greater than vgtg.timeval (probably will be)
                # So the new timeval should be v.timeval + car_time_diff (next_gtg.timeval - vgtg.timeval)
                tval = v.timeval + (next_gtg.timeval - vgtg.timeval)
                new_vert = AerialMAPFState(idx=idx_ctr, ground_transit_idx=next_gtg.idx, timeval=tval, distval=v.distval, considered_cars=v.considered_cars)

                Graphs.add_vertex!(env.aerial_mapf_graph, new_vert)
                push!(nbrs, idx_ctr)

            end

            for (gid, (gstart, gend)) in enumerate(env.gtg_ground_path_start_ends)

                if gid != vgtg.ground_agent_id && ~(gid in v.considered_cars)
                    gts_non_dom = get_non_dominated_transit_points(env, gstart, gend, avoid_gtg_vertex_set, v, true)

                    # Add AerialMAPFState versions of them
                    for gts_tup in gts_non_dom

                        # At least here compute the real thing, the true SP between transit connections
                        # bidir_sp = compute_bidir_astar_euclidean(env.road_graph, vgtg.road_vtx_id, env.ground_transit_graph.vertices[gts_tup.idx].road_vtx_id, env.road_graph_wts, env.location_list)

                        true_timeval = gts_tup.timeval
                        true_distval = gts_tup.distval

                        # Compute the true connection cost between them
                        # if length(bidir_sp.path) > 1
                        #     true_distval = v.distval + bidir_sp.dist
                        #     true_timeval = max(v.timeval + bidir_sp.dist/AvgSpeed, gts_tup.timeval)
                        # end

                        temp_gval = env.alpha_weight_distance*true_distval + (1.0 - env.alpha_weight_distance)*true_timeval

                        if ~haskey(env.gtg_idx_gval, gts_tup.idx) || temp_gval < env.gtg_idx_gval[gts_tup.idx]

                            idx_ctr += 1
                            new_vert = AerialMAPFState(idx=idx_ctr, ground_transit_idx=gts_tup.idx, timeval=true_timeval, distval=true_distval, considered_cars=v.considered_cars)

                            Graphs.add_vertex!(env.aerial_mapf_graph, new_vert)
                            push!(nbrs, idx_ctr)

                            env.gtg_idx_gval[gts_tup.idx] = temp_gval
                        end
                    end
                end
            end # (gstart, gend) in gtg_ground_path_start_ends
        end # vgtg.ground_agent_id != ugtg.ground_agent_id
    end # vgtg.ground == or != 0

    return true
end

function low_level_pp_search!(env::CoordinatedMAPFEnv, aerial_agent_id::Int64, s::AerialMAPFState, constraint::GroundTransitVertexConstraint)

    env.aerial_mapf_graph = SimpleVListGraph{AerialMAPFState}()
    Graphs.add_vertex!(env.aerial_mapf_graph, s)

    env.gtg_idx_gval =  Dict{Int64,Float64}()

    edge_wt_fn(u, v) = amapfg_edge_wt_fn(env, u, v)
    admissible_heuristic(u) = direct_flight_time_heuristic(env, u)

    start_idx = 1   # Always one since graph reinitialized
    env.next_gtg_goal_idx = env.gtg_drone_start_goal_idxs[aerial_agent_id][2]
    env.next_amapfg_goal_idx = 0

    vis = GroundTransitVertexGoalVisitor(env, constraint.avoid_gtg_vertex_set)
    a_star_states = a_star_implicit_shortest_path!(env.aerial_mapf_graph,
                                                    edge_wt_fn,
                                                    start_idx,
                                                    vis,
                                                    admissible_heuristic
                                                    )

    @assert haskey(a_star_states.parent_indices, env.next_amapfg_goal_idx)

    # Get path cost
    states = Tuple{AerialMAPFState,Float64}[]
    actions = Tuple{GroundTransitAction, Float64}[]

    # Get path cost
    sp_cost = a_star_states.dists[env.next_amapfg_goal_idx]

    # Get path
    sp_idxs = shortest_path_indices(a_star_states.parent_indices, env.aerial_mapf_graph, start_idx, env.next_amapfg_goal_idx)

    push!(states, (s, 0.0))

    # Traverse sp_idxs and set up states and actions
    for i = 2:length(sp_idxs)

        u, v =  (sp_idxs[i-1], sp_idxs[i])

        # NOTE: Create new state because we are going to delete graph later
        new_state = deepcopy(env.aerial_mapf_graph.vertices[v])
        push!(states, (new_state, a_star_states.dists[v]))

        # All derived info but can be useful
        push!(actions, (get_mapf_action(env, u, v), a_star_states.dists[v] - a_star_states.dists[u]))
    end

    return PlanResult{AerialMAPFState,GroundTransitAction,Float64}(states, actions, sp_cost, sp_cost)
end # function low_level_search

function add_constraint_from_pp_search!(env::CoordinatedMAPFEnv, constraint::GroundTransitVertexConstraint, aerial_id::Int64, id_path::PlanResult)

    for ((si, _), (sip, _)) in zip(id_path.states[1:end-1], id_path.states[2:end])
        si_gtg = env.ground_transit_graph.vertices[si.ground_transit_idx]
        sip_gtg = env.ground_transit_graph.vertices[sip.ground_transit_idx]
        if si_gtg.ground_agent_id !=0 && si_gtg.ground_agent_id == sip_gtg.ground_agent_id
            if ~haskey(env.gtg_idx_to_aerial_ids, si_gtg.idx)
                env.gtg_idx_to_aerial_ids[si_gtg.idx] = Int64[]
            end
            push!(env.gtg_idx_to_aerial_ids[si_gtg.idx], aerial_id)
        end
    end

    for (gtgidx, aerial_ids) in env.gtg_idx_to_aerial_ids
        if length(aerial_ids) > env.car_capacity
            union!(constraint.avoid_gtg_vertex_set, gtgidx)
        end
    end
end


using .JSort
import PyPlot; const plt = PyPlot
import PyCall
#using Interactive

function GatePicker(matrix, x, y)
    pushfirst!(PyVector(pyimport("sys")."path"), "/home/erdos/gits/JSort/src/")
    interactivegating = pyimport("interactivegating")
    GP = interactivegating.GatePicker
    return GP(matrix, x, y)
end

function GatePicker(det::ParticleDetector; nbins=1000)
    (matrix, (x, y)) = histogram(det, nbins=nbins)
    gp = GatePicker(matrix, x, y)
    return () -> Gate2D(gp.extent...)
end

@enum Action Continue Quit Align
struct ActionEvent
    action::Action
    data::Vector{Any}
end

function ActionEvent(input::AbstractString)
    if input == "q"
        return ActionEvent(Quit, [])
    end
    tokens = split(input, r"\s+")
    if length(tokens) ≥ 2
        try
            data = parse.(Float64, tokens[1:2])
            for token in tokens[3:end]
                key, val = split(token, r"\s*=\s*")
                push!(data, Symbol(key) => parse(Float64, val))
            end
            return ActionEvent(Align, data)
        catch
            return ActionEvent(Continue, [])
        end
    end
    return ActionEvent(Continue, [])
end

quit(event::ActionEvent) = event.action == Quit
align(event::ActionEvent) = event.action == Align

function pickpgate(det::ParticleDetector, parameters::Parameters; nbins=1000)
    quick = QuickReader(parameters)
    read(quick)
    ungated = ChannelDetector(deepcopy(quick))
    char = ""
    default = GammaCalibrator([-913.5, 5.6])
    default(ungated)
    while true
        g = GatePicker(det, nbins=nbins)
        # Pause to let Python write to g
        event = ActionEvent(readline())
        aligner = (x...) -> ()
        if quit(event)
            break
        elseif align(event)
            aligner = (gated) -> begin
                calib = ialign(ungated, event.data[1]:event.data[2], event.data[3:end]...)
                calib(gated)
            end
        end
        @show event

        read(quick, g())
        det2 = ChannelDetector(quick)
        default(det2)
        aligner(det2)
        det2
        plot(det2)
        plot(det2 |> sum)
        plot(det2[1])
    end
end

function ialign(detector::ChannelDetector, region; nbins=5000, kwargs...)
    full, edges = histogram(detector, nbins=nbins)
    chns = goodchannels(detector)

    signal = pyimport("scipy.signal")
    smoother(x) = signal.savgol_filter(x, 51, 4)

    coeff, _ = alignspectra(full, edges, lowregion=region, lowsmoother=smoother,
                            lownumregions=get(kwargs, :nregions, 2),
                            lowregionwidth=get(kwargs, :swidth, 200),
                            lowsearchwidth=get(kwargs, :lwidth, 60),
                            plot=get(kwargs, :plot, false), order=1)
    coefficients = repeat([0.0 1.0], outer=(33, 1))
    coefficients[chns, :] .= hcat(coeff...)'
    calib = GammaCalibrator(coefficients)
end


const mjtNum = Cdouble
const mjMINVAL = 1.0e-15

const mjPI = 3.14159265358979323846
const mjMAXVAL = 1.0e10
const mjMINMU = 1.0e-5
const mjMINIMP = 0.0001
const mjMAXIMP = 0.9999
const mjMAXCONPAIR = 50
const mjMAXVFS = 200
const mjMAXVFSNAME = 100

const mjNEQDATA = 7
const mjNDYN = 10
const mjNGAIN = 10
const mjNBIAS = 10
const mjNREF = 2
const mjNIMP = 5
const mjNSOLVER = 1000

const mjtByte = Cuchar

@cenum mjtDisableBit::Cint begin      # disable default feature bitflags
    mjDSBL_CONSTRAINT = 1 << 0         # entire constraint solver
    mjDSBL_EQUALITY = 1 << 1         # equality constraints
    mjDSBL_FRICTIONLOSS = 1 << 2         # joint and tendon frictionloss constraints
    mjDSBL_LIMIT = 1 << 3         # joint and tendon limit constraints
    mjDSBL_CONTACT = 1 << 4         # contact constraints
    mjDSBL_PASSIVE = 1 << 5         # passive forces
    mjDSBL_GRAVITY = 1 << 6         # gravitational forces
    mjDSBL_CLAMPCTRL = 1 << 7         # clamp control to specified range
    mjDSBL_WARMSTART = 1 << 8         # warmstart constraint solver
    mjDSBL_FILTERPARENT = 1 << 9         # remove collisions with parent body
    mjDSBL_ACTUATION = 1 << 10        # apply actuation forces
    mjDSBL_REFSAFE = 1 << 11        # integrator safety: make ref[0]>=2*timestep

    mjNDISABLE = 12           # number of disable flags
end

@cenum mjtEnableBit::Cint begin       # enable optional feature bitflags
    mjENBL_OVERRIDE = 1 << 0         # override contact parameters
    mjENBL_ENERGY = 1 << 1         # energy computation
    mjENBL_FWDINV = 1 << 2         # record solver statistics
    mjENBL_SENSORNOISE = 1 << 3         # add noise to sensor data

    mjNENABLE = 4          # number of enable flags
end

@cenum mjtJoint::Cint begin           # type of degree of freedom
    mjJNT_FREE = 0            # global position and orientation (quat)       (7)
    mjJNT_BALL                         # orientation (quat) relative to parent        (4)
    mjJNT_SLIDE                        # sliding distance along body-fixed axis       (1)
    mjJNT_HINGE                        # rotation angle (rad) around body-fixed axis  (1)
end

@cenum mjtGeom::Cint begin            # type of geometric shape
    # regular geom types
    mjGEOM_PLANE = 0            # plane
    mjGEOM_HFIELD                      # height field
    mjGEOM_SPHERE                      # sphere
    mjGEOM_CAPSULE                     # capsule
    mjGEOM_ELLIPSOID                   # ellipsoid
    mjGEOM_CYLINDER                    # cylinder
    mjGEOM_BOX                         # box
    mjGEOM_MESH                        # mesh

    mjNGEOMTYPES                       # number of regular geom types

                                     # rendering-only geom types: not used in mjModel not counted in mjNGEOMTYPES
    mjGEOM_ARROW = 100          # arrow
    mjGEOM_ARROW1                      # arrow without wedges
    mjGEOM_ARROW2                      # arrow in both directions
    mjGEOM_LINE                        # line
    mjGEOM_SKIN                        # skin
    mjGEOM_LABEL                       # text label

    mGEOM_NONE = 1001         # missing geom type
end

@cenum mjtCamLight::Cint begin        # tracking mode for camera and light
    mjCAMLIGHT_FIXED = 0            # pos and rot fixed in body
    mjCAMLIGHT_TRACK                   # pos tracks body, rot fixed in global
    mjCAMLIGHT_TRACKCOM                # pos tracks subtree com, rot fixed in body
    mjCAMLIGHT_TARGETBODY              # pos fixed in body, rot tracks target body
    mjCAMLIGHT_TARGETBODYCOM           # pos fixed in body rot tracks target subtree com
end

@cenum mjtTexture::Cint begin         # type of texture
    mjTEXTURE_2D = 0            # 2d texture, suitable for planes and hfields
    mjTEXTURE_CUBE                     # cube texture, suitable for all other geom types
    mjTEXTURE_SKYBOX                   # cube texture used as skybox
end

@cenum mjtIntegrator::Cint begin      # integrator mode
    mjINT_EULER = 0            # semi-implicit Euler
    mjINT_RK4                          # 4th-order Runge Kutta
end

@cenum mjtCollision::Cint begin       # collision mode for selecting geom pairs
    mjCOL_ALL = 0            # test precomputed and dynamic pairs
    mjCOL_PAIR                         # test predefined pairs only
    mjCOL_DYNAMIC                      # test dynamic pairs only
end

@cenum mjtCone::Cint begin            # type of friction cone
    mjCONE_PYRAMIDAL = 0           # pyramidal
    mjCONE_ELLIPTIC                    # elliptic
end

@cenum mjtJacobian::Cint begin        # type of constraint Jacobian
    mjJAC_DENSE = 0           # dense
    mjJAC_SPARSE                       # sparse
    mjJAC_AUTO                         # dense if nv<60 sparse otherwise
end

@cenum mjtSolver::Cint begin          # constraint solver algorithm
    mjSOL_PGS = 0           # PGS    (dual)
    mjSOL_CG                           # CG     (primal)
    mjSOL_NEWTON                       # Newton (primal)
end

@cenum mjtEq::Cint begin              # type of equality constraint
    mjEQ_CONNECT = 0            # connect two bodies at a point (ball joint)
    mjEQ_WELD                          # fix relative position and orientation of two bodies
    mjEQ_JOINT                         # couple the values of two scalar joints with cubic
    mjEQ_TENDON                        # couple the lengths of two tendons with cubic
    mjEQ_DISTANCE                      # fix the contact distance betweent two geoms
end

@cenum mjtWrap::Cint begin            # type of tendon wrap object
    mjWRAP_NONE = 0            # null object
    mjWRAP_JOINT                       # constant moment arm
    mjWRAP_PULLEY                      # pulley used to split tendon
    mjWRAP_SITE                        # pass through site
    mjWRAP_SPHERE                      # wrap around sphere
    mjWRAP_CYLINDER                    # wrap around (infinite) cylinder
end

@cenum mjtTrn::Cint begin             # type of actuator transmission
    mjTRN_JOINT = 0            # force on joint
    mjTRN_JOINTINPARENT                # force on joint, expressed in parent frame
    mjTRN_SLIDERCRANK                  # force via slider-crank linkage
    mjTRN_TENDON                       # force on tendon
    mjTRN_SITE                         # force on site

    mjTRN_UNDEFINED = 1000         # undefined transmission type
end

@cenum mjtDyn::Cint begin             # type of actuator dynamics
    mjDYN_NONE = 0            # no internal dynamics; ctrl specifies force
    mjDYN_INTEGRATOR                   # integrator: da/dt = u
    mjDYN_FILTER                       # linear filter: da/dt = (u-a) / tau
    mjDYN_MUSCLE                       # piece-wise linear filter with two time constants
    mjDYN_USER                         # user-defined dynamics type
end

@cenum mjtGain::Cint begin            # type of actuator gain
    mjGAIN_FIXED = 0            # fixed gain
    mjGAIN_MUSCLE                      # muscle FLV curve computed by mju_muscleGain()
    mjGAIN_USER                        # user-defined gain type
end

@cenum mjtBias::Cint begin            # type of actuator bias
    mjBIAS_NONE = 0            # no bias
    mjBIAS_AFFINE                      # const + kp*length + kv*velocity
    mjBIAS_MUSCLE                      # muscle passive force computed by mju_muscleBias()
    mjBIAS_USER                        # user-defined bias type
end

@cenum mjtObj::Cint begin             # type of MujoCo object
    mjOBJ_UNKNOWN = 0            # unknown object type
    mjOBJ_BODY                         # body
    mjOBJ_XBODY                        # body, used to access regular frame instead of i-frame
    mjOBJ_JOINT                        # joint
    mjOBJ_DOF                          # dof
    mjOBJ_GEOM                         # geom
    mjOBJ_SITE                         # site
    mjOBJ_CAMERA                       # camera
    mjOBJ_LIGHT                        # light
    mjOBJ_MESH                         # mesh
    mjOBJ_SKIN                         # skin
    mjOBJ_HFIELD                       # heightfield
    mjOBJ_TEXTURE                      # texture
    mjOBJ_MATERIAL                     # material for rendering
    mjOBJ_PAIR                         # geom pair to include
    mjOBJ_EXCLUDE                      # body pair to exclude
    mjOBJ_EQUALITY                     # equality constraint
    mjOBJ_TENDON                       # tendon
    mjOBJ_ACTUATOR                     # actuator
    mjOBJ_SENSOR                       # sensor
    mjOBJ_NUMERIC                      # numeric
    mjOBJ_TEXT                         # text
    mjOBJ_TUPLE                        # tuple
    mjOBJ_KEY                          # keyframe
end

@cenum mjtConstraint::Cint begin      # type of constraint
    mjCNSTR_EQUALITY = 0            # equality constraint
    mjCNSTR_FRICTION_DOF               # dof friction
    mjCNSTR_FRICTION_TENDON            # tendon friction
    mjCNSTR_LIMIT_JOINT                # joint limit
    mjCNSTR_LIMIT_TENDON               # tendon limit
    mjCNSTR_CONTACT_FRICTIONLESS       # frictionless contact
    mjCNSTR_CONTACT_PYRAMIDAL          # frictional contact, pyramidal friction cone
    mjCNSTR_CONTACT_ELLIPTIC           # frictional contact elliptic friction cone
end

@cenum mjtConstraintState::Cint begin # constraint state
    mjCNSTRSTATE_SATISFIED = 0         # constraint satisfied, zero cost (limit, contact)
    mjCNSTRSTATE_QUADRATIC             # quadratic cost (equality, friction, limit, contact)
    mjCNSTRSTATE_LINEARNEG             # linear cost, negative side (friction)
    mjCNSTRSTATE_LINEARPOS             # linear cost, positive side (friction)
    mjCNSTRSTATE_CONE                  # squared distance to cone cost (elliptic contact)
end

@cenum mjtSensor::Cint begin          # type of sensor
                                     # common robotic sensors attached to a site
    mjSENS_TOUCH = 0            # scalar contact normal forces summed over sensor zone
    mjSENS_ACCELEROMETER               # 3D linear acceleration, in local frame
    mjSENS_VELOCIMETER                 # 3D linear velocity, in local frame
    mjSENS_GYRO                        # 3D angular velocity, in local frame
    mjSENS_FORCE                       # 3D force between site's body and its parent body
    mjSENS_TORQUE                      # 3D torque between site's body and its parent body
    mjSENS_MAGNETOMETER                # 3D magnetometer
    mjSENS_RANGEFINDER                 # scalar distance to nearest geom or site along z-axis

                                     # sensors related to scalar joints tendons, actuators
    mjSENS_JOINTPOS                    # scalar joint position (hinge and slide only)
    mjSENS_JOINTVEL                    # scalar joint velocity (hinge and slide only)
    mjSENS_TENDONPOS                   # scalar tendon position
    mjSENS_TENDONVEL                   # scalar tendon velocity
    mjSENS_ACTUATORPOS                 # scalar actuator position
    mjSENS_ACTUATORVEL                 # scalar actuator velocity
    mjSENS_ACTUATORFRC                 # scalar actuator force

                                     # sensors related to ball joints
    mjSENS_BALLQUAT                    # 4D ball joint quaterion
    mjSENS_BALLANGVEL                  # 3D ball joint angular velocity

                                     # joint and tendon limit sensors in constraint space
    mjSENS_JOINTLIMITPOS               # joint limit distance-margin
    mjSENS_JOINTLIMITVEL               # joint limit velocity
    mjSENS_JOINTLIMITFRC               # joint limit force
    mjSENS_TENDONLIMITPOS              # tendon limit distance-margin
    mjSENS_TENDONLIMITVEL              # tendon limit velocity
    mjSENS_TENDONLIMITFRC              # tendon limit force

                                     # sensors attached to an object with spatial frame: (x)body geom, site, camera
    mjSENS_FRAMEPOS                    # 3D position
    mjSENS_FRAMEQUAT                   # 4D unit quaternion orientation
    mjSENS_FRAMEXAXIS                  # 3D unit vector: x-axis of object's frame
    mjSENS_FRAMEYAXIS                  # 3D unit vector: y-axis of object's frame
    mjSENS_FRAMEZAXIS                  # 3D unit vector: z-axis of object's frame
    mjSENS_FRAMELINVEL                 # 3D linear velocity
    mjSENS_FRAMEANGVEL                 # 3D angular velocity
    mjSENS_FRAMELINACC                 # 3D linear acceleration
    mjSENS_FRAMEANGACC                 # 3D angular acceleration

                                     # sensors related to kinematic subtrees; attached to a body (which is the subtree root)
    mjSENS_SUBTREECOM                  # 3D center of mass of subtree
    mjSENS_SUBTREELINVEL               # 3D linear velocity of subtree
    mjSENS_SUBTREEANGMOM               # 3D angular momentum of subtree

                                     # user-defined sensor
    mjSENS_USER                        # sensor data provided by mjcb_sensor callback
end

@cenum mjtStage::Cint begin           # computation stage
    mjSTAGE_NONE = 0            # no computations
    mjSTAGE_POS                        # position-dependent computations
    mjSTAGE_VEL                        # velocity-dependent computations
    mjSTAGE_ACC                        # acceleration/force-dependent computations
end

@cenum mjtDataType::Cint begin        # data type for sensors
    mjDATATYPE_REAL = 0            # real values, no constraints
    mjDATATYPE_POSITIVE                # positive values; 0 or negative: inactive
    mjDATATYPE_AXIS                    # 3D unit vector
    mjDATATYPE_QUATERNION              # unit quaternion
end

@cenum mjtLRMode::Cint begin          # mode for actuator length range computation
    mjLRMODE_NONE = 0                # do not process any actuators
    mjLRMODE_MUSCLE                    # process muscle actuators
    mjLRMODE_MUSCLEUSER                # process muscle and user actuators
    mjLRMODE_ALL                       # process all actuators
end

@uninitable mutable struct mjLROpt # options for mj_setLengthRange()
    # flags
    mode::Cint         # which actuators to process (mjtLRMode)
    useexisting::Cint  # use existing length range if available
    uselimit::Cint     # use joint and tendon limits if available

    # algorithm parameters
    accel::mjtNum      # target acceleration used to compute force
    maxforce::mjtNum   # maximum force 0: no limit
    timeconst::mjtNum  # time constant for velocity reduction min 0.01
    timestep::mjtNum   # simulation timestep 0: use mjOption.timestep
    inttotal::mjtNum   # total simulation time interval
    inteval::mjtNum    # evaluation time interval (at the end)
    tolrange::mjtNum   # convergence tolerance (relative to range)
end

@uninitable mutable struct mjVFS
    nfile::Cint
    filename::SVector{mjMAXVFS,SVector{mjMAXVFSNAME,UInt8}}
    filesize::SVector{mjMAXVFS,Cint}
    filedata::SVector{mjMAXVFS,Ptr{Cvoid}}
end

@uninitable struct mjOption
    timestep::mjtNum
    apirate::mjtNum

    impratio::mjtNum
    tolerance::mjtNum
    noslip_tolerance::mjtNum
    mpr_tolerance::mjtNum

    gravity::SVector{3,mjtNum}
    wind::SVector{3,mjtNum}
    magnetic::SVector{3,mjtNum}
    density::mjtNum
    viscosity::mjtNum

    o_margin::mjtNum
    o_solref::SVector{mjNREF,mjtNum}
    o_solimp::SVector{mjNIMP,mjtNum}

    integrator::Cint
    collision::Cint
    cone::Cint
    jacobian::Cint
    solver::Cint
    iterations::Cint
    noslip_iterations::Cint
    mpr_iterations::Cint
    disableflags::Cint
    enableflags::Cint
end

# Julia doesn't allow struct definition's inside of structs like in C,
# so we define the inner structs of mjVisual outside, prefixed with `mjVisual_`
@uninitable struct mjVisual_global
    fovy::Cfloat
    ipd::Cfloat
    linewidth::Cfloat
    glow::Cfloat
    offwidth::Cint
    offheight::Cint
end

@uninitable struct mjVisual_quality
    shadowsize::Cint
    offsamples::Cint
    numslices::Cint
    numstacks::Cint
    numquads::Cint
end

@uninitable struct mjVisual_headlight
    ambient::SVector{3,Cfloat}
    diffuse::SVector{3,Cfloat}
    specular::SVector{3,Cfloat}
    active::Cint
end

@uninitable struct mjVisual_map
    stiffness::Cfloat
    stiffnessrot::Cfloat
    force::Cfloat
    torque::Cfloat
    alpha::Cfloat
    fogstart::Cfloat
    fogend::Cfloat
    znear::Cfloat
    zfar::Cfloat
    haze::Cfloat
    shadowclip::Cfloat
    shadowscale::Cfloat
    actuatortendon::Cfloat
end

@uninitable struct mjVisual_scale
    forcewidth::Cfloat
    contactwidth::Cfloat
    contactheight::Cfloat
    connect::Cfloat
    com::Cfloat
    camera::Cfloat
    light::Cfloat
    selectpoint::Cfloat
    jointlength::Cfloat
    jointwidth::Cfloat
    actuatorlength::Cfloat
    actuatorwidth::Cfloat
    framelength::Cfloat
    framewidth::Cfloat
    constraint::Cfloat
    slidercrank::Cfloat
end

@uninitable struct mjVisual_rgba
    fog::SVector{4,Cfloat}
    haze::SVector{4,Cfloat}
    force::SVector{4,Cfloat}
    inertia::SVector{4,Cfloat}
    joint::SVector{4,Cfloat}
    actuator::SVector{4,Cfloat}
    actuatornegative::SVector{4,Cfloat}
    actuatorpositive::SVector{4,Cfloat}
    com::SVector{4,Cfloat}
    camera::SVector{4,Cfloat}
    light::SVector{4,Cfloat}
    selectpoint::SVector{4,Cfloat}
    connect::SVector{4,Cfloat}
    contactpoint::SVector{4,Cfloat}
    contactforce::SVector{4,Cfloat}
    contactfriction::SVector{4,Cfloat}
    contacttorque::SVector{4,Cfloat}
    contactgap::SVector{4,Cfloat}
    rangefinder::SVector{4,Cfloat}
    constraint::SVector{4,Cfloat}
    slidercrank::SVector{4,Cfloat}
    crankbroken::SVector{4,Cfloat}
end

@uninitable struct mjVisual
    _global::mjVisual_global
    quality::mjVisual_quality
    headlight::mjVisual_headlight
    map::mjVisual_map
    scale::mjVisual_scale
    rgba::mjVisual_rgba
end

@uninitable struct mjStatistic
    meaninertia::mjtNum
    meanmass::mjtNum
    meansize::mjtNum
    extent::mjtNum
    center::SVector{3,mjtNum}
end

# TODO immutable
mutable struct mjModel
    nq::Cint
    nv::Cint
    nu::Cint
    na::Cint
    nbody::Cint
    njnt::Cint
    ngeom::Cint
    nsite::Cint
    ncam::Cint
    nlight::Cint
    nmesh::Cint
    nmeshvert::Cint
    nmeshtexvert::Cint
    nmeshface::Cint
    nmeshgraph::Cint
    nskin::Cint
    nskinvert::Cint
    nskintexvert::Cint
    nskinface::Cint
    nskinbone::Cint
    nskinbonevert::Cint
    nhfield::Cint
    nhfielddata::Cint
    ntex::Cint
    ntexdata::Cint
    nmat::Cint
    npair::Cint
    nexclude::Cint
    neq::Cint
    ntendon::Cint
    nwrap::Cint
    nsensor::Cint
    nnumeric::Cint
    nnumericdata::Cint
    ntext::Cint
    ntextdata::Cint
    ntuple::Cint
    ntupledata::Cint
    nkey::Cint
    nuser_body::Cint
    nuser_jnt::Cint
    nuser_geom::Cint
    nuser_site::Cint
    nuser_cam::Cint
    nuser_tendon::Cint
    nuser_actuator::Cint
    nuser_sensor::Cint
    nnames::Cint

    nM::Cint
    nemax::Cint
    njmax::Cint
    nconmax::Cint
    nstack::Cint
    nuserdata::Cint
    nmocap::Cint
    nsensordata::Cint

    nbuffer::Cint

    opt::mjOption
    vis::mjVisual
    stat::mjStatistic

    buffer::Ptr{Cvoid}

    qpos0::Ptr{mjtNum}
    qpos_spring::Ptr{mjtNum}

    body_parentid::Ptr{Cint}
    body_rootid::Ptr{Cint}
    body_weldid::Ptr{Cint}
    body_mocapid::Ptr{Cint}
    body_jntnum::Ptr{Cint}
    body_jntadr::Ptr{Cint}
    body_dofnum::Ptr{Cint}
    body_dofadr::Ptr{Cint}
    body_geomnum::Ptr{Cint}
    body_geomadr::Ptr{Cint}
    body_simple::Ptr{mjtByte}
    body_sameframe::Ptr{mjtByte}
    body_pos::Ptr{mjtNum}
    body_quat::Ptr{mjtNum}
    body_ipos::Ptr{mjtNum}
    body_iquat::Ptr{mjtNum}
    body_mass::Ptr{mjtNum}
    body_subtreemass::Ptr{mjtNum}
    body_inertia::Ptr{mjtNum}
    body_invweight0::Ptr{mjtNum}
    body_user::Ptr{mjtNum}

    jnt_type::Ptr{Cint}
    jnt_qposadr::Ptr{Cint}
    jnt_dofadr::Ptr{Cint}
    jnt_bodyid::Ptr{Cint}
    jnt_group::Ptr{Cint}
    jnt_limited::Ptr{mjtByte}
    jnt_solref::Ptr{mjtNum}
    jnt_solimp::Ptr{mjtNum}
    jnt_pos::Ptr{mjtNum}
    jnt_axis::Ptr{mjtNum}
    jnt_stiffness::Ptr{mjtNum}
    jnt_range::Ptr{mjtNum}
    jnt_margin::Ptr{mjtNum}
    jnt_user::Ptr{mjtNum}

    dof_bodyid::Ptr{Cint}
    dof_jntid::Ptr{Cint}
    dof_parentid::Ptr{Cint}
    dof_Madr::Ptr{Cint}
    dof_simplenum::Ptr{Cint}
    dof_solref::Ptr{mjtNum}
    dof_solimp::Ptr{mjtNum}
    dof_frictionloss::Ptr{mjtNum}
    dof_armature::Ptr{mjtNum}
    dof_damping::Ptr{mjtNum}
    dof_invweight0::Ptr{mjtNum}
    dof_M0::Ptr{mjtNum}

    geom_type::Ptr{Cint}
    geom_contype::Ptr{Cint}
    geom_conaffinity::Ptr{Cint}
    geom_condim::Ptr{Cint}
    geom_bodyid::Ptr{Cint}
    geom_dataid::Ptr{Cint}
    geom_matid::Ptr{Cint}
    geom_group::Ptr{Cint}
    geom_priority::Ptr{Cint}
    geom_sameframe::Ptr{mjtByte}
    geom_solmix::Ptr{mjtNum}
    geom_solref::Ptr{mjtNum}
    geom_solimp::Ptr{mjtNum}
    geom_size::Ptr{mjtNum}
    geom_rbound::Ptr{mjtNum}
    geom_pos::Ptr{mjtNum}
    geom_quat::Ptr{mjtNum}
    geom_friction::Ptr{mjtNum}
    geom_margin::Ptr{mjtNum}
    geom_gap::Ptr{mjtNum}
    geom_user::Ptr{mjtNum}
    geom_rgba::Ptr{Cfloat}

    site_type::Ptr{Cint}
    site_bodyid::Ptr{Cint}
    site_matid::Ptr{Cint}
    site_group::Ptr{Cint}
    site_sameframe::Ptr{mjtByte}
    site_size::Ptr{mjtNum}
    site_pos::Ptr{mjtNum}
    site_quat::Ptr{mjtNum}
    site_user::Ptr{mjtNum}
    site_rgba::Ptr{Cfloat}

    cam_mode::Ptr{Cint}
    cam_bodyid::Ptr{Cint}
    cam_targetbodyid::Ptr{Cint}
    cam_pos::Ptr{mjtNum}
    cam_quat::Ptr{mjtNum}
    cam_poscom0::Ptr{mjtNum}
    cam_pos0::Ptr{mjtNum}
    cam_mat0::Ptr{mjtNum}
    cam_fovy::Ptr{mjtNum}
    cam_ipd::Ptr{mjtNum}
    cam_user::Ptr{mjtNum}

    light_mode::Ptr{Cint}
    light_bodyid::Ptr{Cint}
    light_targetbodyid::Ptr{Cint}
    light_directional::Ptr{mjtByte}
    light_castshadow::Ptr{mjtByte}
    light_active::Ptr{mjtByte}
    light_pos::Ptr{mjtNum}
    light_dir::Ptr{mjtNum}
    light_poscom0::Ptr{mjtNum}
    light_pos0::Ptr{mjtNum}
    light_dir0::Ptr{mjtNum}
    light_attenuation::Ptr{Cfloat}
    light_cutoff::Ptr{Cfloat}
    light_exponent::Ptr{Cfloat}
    light_ambient::Ptr{Cfloat}
    light_diffuse::Ptr{Cfloat}
    light_specular::Ptr{Cfloat}

    mesh_vertadr::Ptr{Cint}
    mesh_vertnum::Ptr{Cint}
    mesh_texcoordadr::Ptr{Cint}
    mesh_faceadr::Ptr{Cint}
    mesh_facenum::Ptr{Cint}
    mesh_graphadr::Ptr{Cint}
    mesh_vert::Ptr{Cfloat}
    mesh_normal::Ptr{Cfloat}
    mesh_texcoord::Ptr{Cfloat}
    mesh_face::Ptr{Cint}
    mesh_graph::Ptr{Cint}

    skin_matid::Ptr{Cint}
    skin_rgba::Ptr{Cfloat}
    skin_inflate::Ptr{Cfloat}
    skin_vertadr::Ptr{Cint}
    skin_vertnum::Ptr{Cint}
    skin_texcoordadr::Ptr{Cint}
    skin_faceadr::Ptr{Cint}
    skin_facenum::Ptr{Cint}
    skin_boneadr::Ptr{Cint}
    skin_bonenum::Ptr{Cint}
    skin_vert::Ptr{Cfloat}
    skin_texcoord::Ptr{Cfloat}
    skin_face::Ptr{Cint}
    skin_bonevertadr::Ptr{Cint}
    skin_bonevertnum::Ptr{Cint}
    skin_bonebindpos::Ptr{Cfloat}
    skin_bonebindquat::Ptr{Cfloat}
    skin_bonebodyid::Ptr{Cint}
    skin_bonevertid::Ptr{Cint}
    skin_bonevertweight::Ptr{Cfloat}

    hfield_size::Ptr{mjtNum}
    hfield_nrow::Ptr{Cint}
    hfield_ncol::Ptr{Cint}
    hfield_adr::Ptr{Cint}
    hfield_data::Ptr{Cfloat}

    tex_type::Ptr{Cint}
    tex_height::Ptr{Cint}
    tex_width::Ptr{Cint}
    tex_adr::Ptr{Cint}
    tex_rgb::Ptr{mjtByte}

    mat_texid::Ptr{Cint}
    mat_texuniform::Ptr{mjtByte}
    mat_texrepeat::Ptr{Cfloat}
    mat_emission::Ptr{Cfloat}
    mat_specular::Ptr{Cfloat}
    mat_shininess::Ptr{Cfloat}
    mat_reflectance::Ptr{Cfloat}
    mat_rgba::Ptr{Cfloat}

    pair_dim::Ptr{Cint}
    pair_geom1::Ptr{Cint}
    pair_geom2::Ptr{Cint}
    pair_signature::Ptr{Cint}
    pair_solref::Ptr{mjtNum}
    pair_solimp::Ptr{mjtNum}
    pair_margin::Ptr{mjtNum}
    pair_gap::Ptr{mjtNum}
    pair_friction::Ptr{mjtNum}

    exclude_signature::Ptr{Cint}

    eq_type::Ptr{Cint}
    eq_obj1id::Ptr{Cint}
    eq_obj2id::Ptr{Cint}
    eq_active::Ptr{mjtByte}
    eq_solref::Ptr{mjtNum}
    eq_solimp::Ptr{mjtNum}
    eq_data::Ptr{mjtNum}

    tendon_adr::Ptr{Cint}
    tendon_num::Ptr{Cint}
    tendon_matid::Ptr{Cint}
    tendon_group::Ptr{Cint}
    tendon_limited::Ptr{mjtByte}
    tendon_width::Ptr{mjtNum}
    tendon_solref_lim::Ptr{mjtNum}
    tendon_solimp_lim::Ptr{mjtNum}
    tendon_solref_fri::Ptr{mjtNum}
    tendon_solimp_fri::Ptr{mjtNum}
    tendon_range::Ptr{mjtNum}
    tendon_margin::Ptr{mjtNum}
    tendon_stiffness::Ptr{mjtNum}
    tendon_damping::Ptr{mjtNum}
    tendon_frictionloss::Ptr{mjtNum}
    tendon_lengthspring::Ptr{mjtNum}
    tendon_length0::Ptr{mjtNum}
    tendon_invweight0::Ptr{mjtNum}
    tendon_user::Ptr{mjtNum}
    tendon_rgba::Ptr{Cfloat}

    wrap_type::Ptr{Cint}
    wrap_objid::Ptr{Cint}
    wrap_prm::Ptr{mjtNum}

    actuator_trntype::Ptr{Cint}
    actuator_dyntype::Ptr{Cint}
    actuator_gaintype::Ptr{Cint}
    actuator_biastype::Ptr{Cint}
    actuator_trnid::Ptr{Cint}
    actuator_group::Ptr{Cint}
    actuator_ctrllimited::Ptr{mjtByte}
    actuator_forcelimited::Ptr{mjtByte}
    actuator_dynprm::Ptr{mjtNum}
    actuator_gainprm::Ptr{mjtNum}
    actuator_biasprm::Ptr{mjtNum}
    actuator_ctrlrange::Ptr{mjtNum}
    actuator_forcerange::Ptr{mjtNum}
    actuator_gear::Ptr{mjtNum}
    actuator_cranklength::Ptr{mjtNum}
    actuator_acc0::Ptr{mjtNum}
    actuator_length0::Ptr{mjtNum}
    actuator_lengthrange::Ptr{mjtNum}
    actuator_user::Ptr{mjtNum}

    sensor_type::Ptr{Cint}
    sensor_datatype::Ptr{Cint}
    sensor_needstage::Ptr{Cint}
    sensor_objtype::Ptr{Cint}
    sensor_objid::Ptr{Cint}
    sensor_dim::Ptr{Cint}
    sensor_adr::Ptr{Cint}
    sensor_cutoff::Ptr{mjtNum}
    sensor_noise::Ptr{mjtNum}
    sensor_user::Ptr{mjtNum}

    numeric_adr::Ptr{Cint}
    numeric_size::Ptr{Cint}
    numeric_data::Ptr{mjtNum}

    text_adr::Ptr{Cint}
    text_size::Ptr{Cint}
    text_data::Ptr{UInt8}

    tuple_adr::Ptr{Cint}
    tuple_size::Ptr{Cint}
    tuple_objtype::Ptr{Cint}
    tuple_objid::Ptr{Cint}
    tuple_objprm::Ptr{mjtNum}

    key_time::Ptr{mjtNum}
    key_qpos::Ptr{mjtNum}
    key_qvel::Ptr{mjtNum}
    key_act::Ptr{mjtNum}

    name_bodyadr::Ptr{Cint}
    name_jntadr::Ptr{Cint}
    name_geomadr::Ptr{Cint}
    name_siteadr::Ptr{Cint}
    name_camadr::Ptr{Cint}
    name_lightadr::Ptr{Cint}
    name_meshadr::Ptr{Cint}
    name_skinadr::Ptr{Cint}
    name_hfieldadr::Ptr{Cint}
    name_texadr::Ptr{Cint}
    name_matadr::Ptr{Cint}
    name_pairadr::Ptr{Cint}
    name_excludeadr::Ptr{Cint}
    name_eqadr::Ptr{Cint}
    name_tendonadr::Ptr{Cint}
    name_actuatoradr::Ptr{Cint}
    name_sensoradr::Ptr{Cint}
    name_numericadr::Ptr{Cint}
    name_textadr::Ptr{Cint}
    name_tupleadr::Ptr{Cint}
    name_keyadr::Ptr{Cint}
    names::Ptr{UInt8}
end


using CSyntax
using Quaternions
using GLTF

@static if Sys.isapple()
    const VERSION_MAJOR = 4
    const VERSION_MINOR = 1
end

include(joinpath(@__DIR__, "glutils.jl"))
include(joinpath(@__DIR__, "camera.jl"))

# init window
width, height = fb_width, fb_height = 640, 480
window = startgl(width, height)

glEnable(GL_DEPTH_TEST)
glDepthFunc(GL_LESS)

# we use glTF file format because it's OpenGL friendly and we don't need to care about
# how to parse and extract mesh data from other mesh file format like .obj format.
# load glTF file
sphere = GLTF.load(joinpath(@__DIR__, "sphere.gltf"))
sphere_data = [read(joinpath(@__DIR__, b.uri)) for b in sphere.buffers]

# load sphere position metadata
search_name(x, keyword) = x[findfirst(x->occursin(keyword, x.name), x)]
pos = search_name(sphere.accessors, "position")
pos_bv = sphere.bufferViews[pos.bufferView]
# load sphere index metadata
indices = search_name(sphere.accessors, "indices")
idx_bv = sphere.bufferViews[indices.bufferView]

# create VBO and EBO
vbo = GLuint(0)
@c glGenBuffers(1, &vbo)
glBindBuffer(pos_bv.target, vbo)
glBufferData(pos_bv.target, pos_bv.byteLength, C_NULL, GL_STATIC_DRAW)
pos_data = sphere_data[pos_bv.buffer]
@c glBufferSubData(pos_bv.target, 0, pos_bv.byteLength, &pos_data[pos_bv.byteOffset])

ebo = GLuint(0)
@c glGenBuffers(1, &ebo)
glBindBuffer(idx_bv.target, ebo)
glBufferData(idx_bv.target, idx_bv.byteLength, C_NULL, GL_STATIC_DRAW)
idx_data = sphere_data[idx_bv.buffer]
@c glBufferSubData(idx_bv.target, 0, idx_bv.byteLength, &idx_data[idx_bv.byteOffset])

# create VAO
vao = GLuint(0)
@c glGenVertexArrays(1, &vao)
glBindVertexArray(vao)
glBindBuffer(pos_bv.target, vbo)
glVertexAttribPointer(0, 3, pos.componentType, pos.normalized, pos_bv.byteStride, Ptr{Cvoid}(pos.byteOffset))
glEnableVertexAttribArray(0)

# load and compile shaders from file
vert_shader = createshader(joinpath(@__DIR__, "quat.vert"), GL_VERTEX_SHADER)
frag_shader = createshader(joinpath(@__DIR__, "quat.frag"), GL_FRAGMENT_SHADER)

# link program
shader_prog = createprogram(vert_shader, frag_shader)

# enable cull face
glEnable(GL_CULL_FACE)
glCullFace(GL_BACK)
glFrontFace(GL_CCW)
# set background color to gray
glClearColor(0.2, 0.2, 0.2, 1.0)

# camera
camera = PerspectiveCamera()
setposition!(camera, [0.0, 0.0, 5.0])
model_loc = glGetUniformLocation(shader_prog, "model")
view_loc = glGetUniformLocation(shader_prog, "view")
proj_loc = glGetUniformLocation(shader_prog, "proj")
glUseProgram(shader_prog)
glUniformMatrix4fv(view_loc, 1, GL_FALSE, get_view_matrix(camera))
glUniformMatrix4fv(proj_loc, 1, GL_FALSE, get_projective_matrix(window, camera))

# spheres in world
sphere_world = GLfloat[-2.0 0.0  0.0;
                        2.0 0.0  0.0;
                       -2.0 0.0 -2.0;
                        1.5 1.0 -1.0]
model_mats = map(1:4) do i
    GLfloat[ 1.0 0.0 0.0 sphere_world[i,1];
             0.0 1.0 0.0 sphere_world[i,2];
             0.0 0.0 1.0 sphere_world[i,3];
             0.0 0.0 0.0       1.0        ]
end

let
updatefps = FPSCounter()
while !GLFW.WindowShouldClose(window)
    updatefps(window)
    # clear drawing surface
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
    glViewport(0, 0, GLFW.GetFramebufferSize(window)...)
    # drawing
    glUseProgram(shader_prog)
    glBindVertexArray(vao)
    glBindBuffer(idx_bv.target, ebo)
    for i = 1:4
        glUniformMatrix4fv(model_loc, 1, GL_FALSE, model_mats[i])
        glDrawElements(GL_TRIANGLES, indices.count, indices.componentType, Ptr{Cvoid}(0))
    end
    # check and call events
    GLFW.PollEvents()
    # move camera
    updatecamera!(window, camera)
    glUniformMatrix4fv(view_loc, 1, GL_FALSE, get_view_matrix(camera))
    # swap the buffers
    GLFW.SwapBuffers(window)
end
end # let

GLFW.DestroyWindow(window)


function EstimatedTroposphericModel(arg0::MappingFunction, arg1::jdouble)
    return EstimatedTroposphericModel((MappingFunction, jdouble), arg0, arg1)
end

function EstimatedTroposphericModel(arg0::jdouble, arg1::jdouble, arg2::MappingFunction, arg3::jdouble)
    return EstimatedTroposphericModel((jdouble, jdouble, MappingFunction, jdouble), arg0, arg1, arg2, arg3)
end

function compute_zenith_delay(obj::EstimatedTroposphericModel, arg0::RealFieldElement, arg1::Vector{RealFieldElement}, arg2::FieldAbsoluteDate)
    return jcall(obj, "computeZenithDelay", Vector{RealFieldElement}, (RealFieldElement, Vector{RealFieldElement}, FieldAbsoluteDate), arg0, arg1, arg2)
end

function compute_zenith_delay(obj::EstimatedTroposphericModel, arg0::jdouble, arg1::Vector{jdouble}, arg2::AbsoluteDate)
    return jcall(obj, "computeZenithDelay", Vector{jdouble}, (jdouble, Vector{jdouble}, AbsoluteDate), arg0, arg1, arg2)
end

function equals(obj::Object, arg0::Object)
    return jcall(obj, "equals", jboolean, (Object,), arg0)
end

function get_class(obj::Object)
    return jcall(obj, "getClass", Class, ())
end

function get_parameters_drivers(obj::EstimatedTroposphericModel)
    return jcall(obj, "getParametersDrivers", List, ())
end

function hash_code(obj::Object)
    return jcall(obj, "hashCode", jint, ())
end

function mapping_factors(obj::EstimatedTroposphericModel, arg0::RealFieldElement, arg1::RealFieldElement, arg2::Vector{RealFieldElement}, arg3::FieldAbsoluteDate)
    return jcall(obj, "mappingFactors", Vector{RealFieldElement}, (RealFieldElement, RealFieldElement, Vector{RealFieldElement}, FieldAbsoluteDate), arg0, arg1, arg2, arg3)
end

function mapping_factors(obj::EstimatedTroposphericModel, arg0::jdouble, arg1::jdouble, arg2::Vector{jdouble}, arg3::AbsoluteDate)
    return jcall(obj, "mappingFactors", Vector{jdouble}, (jdouble, jdouble, Vector{jdouble}, AbsoluteDate), arg0, arg1, arg2, arg3)
end

function notify(obj::Object)
    return jcall(obj, "notify", void, ())
end

function notify_all(obj::Object)
    return jcall(obj, "notifyAll", void, ())
end

function path_delay(obj::EstimatedTroposphericModel, arg0::RealFieldElement, arg1::RealFieldElement, arg2::Vector{RealFieldElement}, arg3::FieldAbsoluteDate)
    return jcall(obj, "pathDelay", RealFieldElement, (RealFieldElement, RealFieldElement, Vector{RealFieldElement}, FieldAbsoluteDate), arg0, arg1, arg2, arg3)
end

function path_delay(obj::EstimatedTroposphericModel, arg0::jdouble, arg1::jdouble, arg2::Vector{jdouble}, arg3::AbsoluteDate)
    return jcall(obj, "pathDelay", jdouble, (jdouble, jdouble, Vector{jdouble}, AbsoluteDate), arg0, arg1, arg2, arg3)
end

function to_string(obj::Object)
    return jcall(obj, "toString", JString, ())
end

function wait(obj::Object)
    return jcall(obj, "wait", void, ())
end

function wait(obj::Object, arg0::jlong)
    return jcall(obj, "wait", void, (jlong,), arg0)
end

function wait(obj::Object, arg0::jlong, arg1::jint)
    return jcall(obj, "wait", void, (jlong, jint), arg0, arg1)
end