# 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.

@with_kw mutable struct ParametricGomoryData{T <: AbstractFloat}
    relaxation::Union{Nothing, SecondStageLP{T}} = nothing
    cglp::Union{Nothing, JuMP.Model} = nothing
    unrelax::Function = () -> nothing
end

@with_kw mutable struct ParametricGomoryParameters
    maximum_iterations::Int = 1
    optimizer = nothing
    strategy::ParametricGomoryStrategy = Gomory()
end
"""
    ParametricGomoryMaster

Master functor object for using weak optimality cuts in an integer L-shaped algorithm. Requires all first-stage decisions to be binary. Create by supplying a [`UseParametricGomory`](@ref) object through `integer_strategy` in `LShaped.Optimizer` or set the [`IntegerStrategy`](@ref) attribute.

"""
struct ParametricGomoryMaster <: AbstractIntegerAlgorithm
    integer_variables::Vector{MOI.VariableIndex}
end

function initialize_integer_algorithm!(::ParametricGomoryMaster, first_stage::JuMP.Model)
    # Sanity check
    if all(is_binary, all_decision_variables(first_stage, 1))
       @warn "All first-stage decisions are binary. Convexification or CombinatorialCuts are more approriate integer strategies"
    end
    # Gather integer variables
    append!(master.integer_variables, gather_integer_variables(subproblem))
    # Sanity check
    if isempty(master.integer_variables)
        @warn "No integer variables in subproblem $(subproblem.id). Integer strategy is superfluous."
    end
    return nothing
end

function handle_integrality!(lshaped::AbstractLShaped, ::ParametricGomoryMaster)
    @unpack τ = lshaped.parameters
    if gap(lshaped) <= τ
        set_metadata!(lshaped.execution.metadata, :converged, true)
    end
    return nothing
end
"""
    ParametricGomoryWorker

Worker functor object for using weak optimality cuts in an integer L-shaped algorithm. Create by supplying a [`UseParametricGomory`](@ref) object through `integer_strategy` in `LShaped.Optimizer` or set the [`IntegerStrategy`](@ref) attribute.

"""
struct ParametricGomoryWorker{T <: AbstractFloat} <: AbstractIntegerAlgorithm
    data::ParametricGomoryData{T}
    parameters::ParametricGomoryParameters
    integer_variables::Vector{MOI.VariableIndex}
    cutting_planes::Vector{MOI.ConstraintIndex}

    function ParametricGomoryWorker(::Type{T}; kw...) where T <: AbstractFloat
        T_ = promote_type(T, Float32)
        worker = new{T_}(ParametricGomoryData{T_}(),
                         ParametricGomoryParameters(; kw...),
                         Vector{MOI.VariableIndex}(),
                         Vector{MOI.ConstraintIndex}())
        return worker
    end
end

function initialize_integer_algorithm!(worker::ParametricGomoryWorker, subproblem::SubProblem)
    # Sanity check
    if any(is_integer, all_decision_variables(subproblem.model, StochasticPrograms.stage(subproblem.model))) ||
       any(is_integer, all_auxiliary_variables(subproblem.model))
        if !(worker.parameters.strategy isa Gomory)
            if subproblem.id == 1
                @warn "Switching convexification strategy to Gomory to handle integer variables."
            end
            worker.parameters.strategy = Gomory()
        end
    end
    # Gather integer variables
    append!(worker.integer_variables, gather_integer_variables(subproblem))
    # Sanity check
    if isempty(worker.integer_variables)
        @warn "No integer variables in subproblem $(subproblem.id). Integer strategy is superfluous."
    end
    # Relax integer restrictions
    worker.data.unrelax = relax_decision_integrality(subproblem.model)
    # Extract model information
    standard_form = worker.parameters.strategy isa Gomory
    worker.data.relaxation = SecondStageLP(subproblem; standard_form = standard_form)
    return nothing
end

function restore!(model::MOI.ModelLike, worker::ParametricGomoryWorker)
    # Delete any added cutting planes
    if !isempty(worker.cutting_planes)
        MOI.delete(model, worker.cutting_planes)
    end
    # Restore integral restrictions
    worker.data.unrelax()
    return nothing
end

function integer_variables(worker::ParametricGomoryWorker)
    return worker.integer_variables
end

function solve_subproblem(subproblem::SubProblem{T},
                          metadata,
                          ::NoFeasibilityAlgorithm,
                          worker::ParametricGomoryWorker,
                          x::AbstractVector) where T <: AbstractFloat
    # Initial solve
    cut = solve_subproblem(subproblem, x)
    # Check if integer restrictions are satisfied
    # and collect any fractional variables
    fractional = MOI.VariableIndex[]
    set_metadata!(metadata,
                  subproblem.id,
                  :integral_solution,
                  check_integrality_restrictions(subproblem, fractional))
    # Check if solution satisfies integer requirements
    if isempty(fractional)
        # No cutting planes needed, just return cut
        return cut
    end
    # Check if L-shaped has converged
    if has_metadata(metadata, :converged)
        # Solve subproblem with integer restrictions
        return solve_unrelaxed(subproblem, metadata, worker, x)
    end
    # Otherwise, add cutting planes until solution is integer
    # or user-limit is reached
    num_iterations = 1
    while true
        inds = MOI.get(subproblem.optimizer, MOI.ListOfVariableIndices())
        ȳ = MOI.get.(subproblem.optimizer, MOI.VariablePrimal(), inds[length(x)+1:end])
        try
            # Generate and add cutting plane
            πx, πy, g = cutting_plane(worker,
                                      worker.parameters.strategy,
                                      subproblem,
                                      x,
                                      ȳ,
                                      fractional)
            f, set, coeffs = moi_constraint(subproblem, πx, πy, g)
            # Add cutting plane to subproblem
            ci = MOI.add_constraint(subproblem.optimizer, f, set)
            push!(worker.cutting_planes, ci)
            # Update subproblem
            if !isempty(coeffs)
                push!(subproblem.linking_constraints, ci)
                push!(subproblem.masterterms, coeffs)
            end
        catch
            @warn "Cutting plane could not be generated in subproblem $(subproblem.id)"
            # If cutting plane could not be generated,
            # return latest working cut and try again next iteration
            return cut
        end
        # Re-solve subproblem
        cut = solve_subproblem(subproblem, x)
        fractional = MOI.VariableIndex[]
        set_metadata!(metadata,
                      subproblem.id,
                      :integral_solution,
                      check_integrality_restrictions(subproblem, fractional))
        if isempty(fractional) || num_iterations >= worker.parameters.maximum_iterations
            # Return cut
            return cut
        end
        # Increment iteration count
        num_iterations += 1
    end
end

function solve_subproblem(subproblem::SubProblem,
                          metadata,
                          feasibility_algorithm::FeasibilityCutsWorker,
                          worker::ParametricGomoryWorker,
                          x::AbstractVector)
    # Prepare auxiliary problem
    model = subproblem.optimizer
    if !prepared(feasibility_algorithm)
        prepare!(model, feasibility_algorithm)
    end
    # Relax integrality
    unrelax = relax_decision_integrality(subproblem.model)
    # Optimize auxiliary problem
    MOI.optimize!(model)
    # Sanity check that aux problem could be solved
    status = MOI.get(subproblem.optimizer, MOI.TerminationStatus())
    if !(status ∈ AcceptableTermination)
        error("Subproblem $(subproblem.id) was not solved properly during feasibility check, returned status code: $status")
    end
    # check objective
    sense = MOI.get(subproblem.optimizer, MOI.ObjectiveSense())
    correction = (sense == MOI.MIN_SENSE || sense == MOI.FEASIBILITY_SENSE) ? 1.0 : -1.0
    w = correction * MOI.get(model, MOI.ObjectiveValue())
    # Ensure correction is available in master
    set_metadata!(metadata, subproblem.id, :correction, correction)
    # Check feasibility
    if w > sqrt(eps())
        # Subproblem is infeasible, create feasibility cut
        cut = FeasibilityCut(subproblem, x)
        # Unrelax integrality restrictions again
        unrelax()
        return cut
    end
    # Unrelax integrality restrictions again
    unrelax()
    # Restore subproblem and solve as usual
    restore_subproblem!(subproblem)
    return solve_subproblem(subproblem, metadata, NoFeasibilityAlgorithm(), worker, x)
end

function solve_unrelaxed(subproblem::SubProblem, metadata, worker::ParametricGomoryWorker, x::AbstractVector)
    # Delete any added cutting planes
    if !isempty(worker.cutting_planes)
        MOI.delete(subproblem.optimizer, worker.cutting_planes)
    end
    empty!(worker.cutting_planes)
    # Restore integral restrictions
    worker.data.unrelax()
    worker.data.unrelax = () -> nothing
    # Solve subproblem with integer restrictions
    MOI.optimize!(subproblem.optimizer)
    status = MOI.get(subproblem.optimizer, MOI.TerminationStatus())
    if status ∈ AcceptableTermination
        # Integer restrictions are satisfied if optimal
        set_metadata!(metadata,
                      subproblem.id,
                      :integral_solution,
                      true)
        # Get sense
        sense = MOI.get(subproblem.optimizer, MOI.ObjectiveSense())
        correction = (sense == MOI.MIN_SENSE || sense == MOI.FEASIBILITY_SENSE) ? 1.0 : -1.0
        # Create sense-corrected optimality cut
        Q = correction * MOI.get(subproblem.optimizer, MOI.ObjectiveValue())
        return OptimalityCut(spzeros(length(x)), Q, subproblem.id)
    elseif status == MOI.INFEASIBLE
        return Infeasible(subproblem)
    elseif status == MOI.DUAL_INFEASIBLE
        return Unbounded(subproblem)
    else
        error("Subproblem $(subproblem.id) was not solved properly, returned status code: $status")
    end
end

function moi_constraint(subproblem::SubProblem{T}, πx::AbstractVector, πy::AbstractVector, g::AbstractFloat) where T <: AbstractFloat
    # Create cutting plane
    f = zero(AffineDecisionFunction{T})
    coeffs = Vector{Tuple{Int, Int, T}}()
    N = StochasticPrograms.stage(subproblem.model)
    all_known = mapreduce(vcat, 1:N-1) do s
        index.(all_known_decision_variables(subproblem.model, s))
    end
    all_decisions = index.(all_decision_variables(subproblem.model, N))
    ix = 0
    iy = 0
    for vi in MOI.get(subproblem.optimizer, MOI.ListOfVariableIndices())
        if vi in all_known
            ix += 1
            coeff = T(πx[ix])
            if abs(coeff) >= sqrt(eps())
                push!(f.known_part.terms, MOI.ScalarAffineTerm(coeff, vi))
                push!(coeffs, (1, vi.value, coeff))
            else
                πx[ix] = zero(T)
            end
        elseif vi in all_decisions
            iy += 1
            coeff = T(πy[iy])
            if abs(coeff) >= sqrt(eps())
                push!(f.decision_part.terms, MOI.ScalarAffineTerm(coeff, vi))
            else
                πy[iy] = zero(T)
            end
        else
            iy += 1
            coeff = T(πy[iy])
            if abs(coeff) >= sqrt(eps())
                push!(f.variable_part.terms, MOI.ScalarAffineTerm(coeff, vi))
            else
                πy[iy] = zero(T)
            end
        end
    end
    set = MOI.GreaterThan{Float64}(g)
    return f, set, coeffs
end

function cutting_plane(worker::ParametricGomoryWorker,
                       ::Gomory,
                       subproblem::SubProblem,
                       x̄::AbstractVector,
                       ȳ::AbstractVector,
                       fractional::Vector{MOI.VariableIndex})
    # Pick smallest fractional index
    fractional_index = fractional[1].value - length(x̄)
    # Get matrices
    T = worker.data.relaxation.T
    W = worker.data.relaxation.W
    h = worker.data.relaxation.h
    ny = length(ȳ)
    # Get current optimal basis
    basis = optimal_basis(subproblem)
    nonbasis = setdiff(1:size(W,2),basis)
    # Compute row of basis inverse corresponding
    # to fractional index
    i = something(findfirst(j -> j == fractional_index, basis), 0)
    if i == 0
        error("Fractional variable not in basis.")
    end
    ei = zeros(size(W, 1))
    ei[i] = 1.0
    Bᵢ⁻¹ = W[:,basis]' \ ei
    # Get source rows
    nonbasic = setdiff(1:ny, basis)
    nonbasic_slack = setdiff(ny+1:size(W,2), basis)
    w = (Bᵢ⁻¹'*W[:,nonbasis])
    γ = (Bᵢ⁻¹'*T)[:]
    ρ = Bᵢ⁻¹'*h
    # Translate x to the origin
    for (j,x) in enumerate(x̄)
        if x == 1.
            ρ -= γ[j]
            γ[j] *= -1
        end
    end
    # Calculate Gomory coefficients
    ϕ(x) = x - floor(x)
    #πx = ϕ.(γ)
    #w = ϕ.(w)
    #ρ = ϕ(ρ)
    ϕγ = ϕ.(γ)
    ϕw = ϕ.(w)
    ρ  = ϕ(ρ)
    πx = min.(ϕγ, (ρ .* (1 .- ϕγ) ./ (1 .- ρ)))
    w  = min.(ϕw, (ρ .* (1 .- ϕw) ./ (1 .- ρ)))
    # Translate back
    for (j,x) in enumerate(x̄)
        if x == 1.
            ρ -= πx[j]
            πx[j] *= -1
        end
    end
    πy = zero(ȳ)
    πy[nonbasic] .= w[nonbasic]
    # Map slacks to rows
    for (coeff,slack) in zip(w[length(nonbasic)+1:end], nonbasic_slack)
        row = slack - ny
        πx .-= W[row,slack] * coeff * T[row,:]
        πy .-= W[row,slack] * coeff * W[row,1:ny]
        ρ -= W[row,slack] * coeff * h[row]
    end
    # Extend standard form LP with new cuts
    add_row!(worker.data.relaxation, πx, πy, ρ; standard_form = true)
    # Return cutting plane
    return πx, πy, ρ
end

function cutting_plane(worker::ParametricGomoryWorker,
                       ::LiftAndProject,
                       subproblem::SubProblem{T},
                       x̄::AbstractVector,
                       ȳ::AbstractVector,
                       fractional::Vector{MOI.VariableIndex}) where T <: AbstractFloat
    # Check optimizer
    if worker.parameters.optimizer === nothing
        error("Cannot generate CGLP without optimizer.")
    end
    # Largest fractional index
    fractional_index = fractional[end].value - length(x̄)
    model = subproblem.model
    nx = length(x̄)
    ny = length(ȳ)
    nw = nx + ny - 1
    # Prepare data
    Gᵢ = Vector{Int}()
    Gⱼ = Vector{Int}()
    Gᵥ = Vector{T}()
    Hᵢ = Vector{Int}()
    Hⱼ = Vector{Int}()
    Hᵥ = Vector{T}()
    Fᵢ = Vector{Int}()
    Fⱼ = Vector{Int}()
    Fᵥ = Vector{T}()
    lhs = Vector{T}()
    rhs = Vector{T}()
    i = 0
    # Loop over constraints
    for (F,S) in list_of_constraint_types(model)
        for cref in all_constraints(model, F, S)
            if S <: StochasticPrograms.SingleDecisionSet ||
                S <: StochasticPrograms.MultipleDecisionSet ||
                S <: StochasticPrograms.SingleKnownSet ||
                S <: StochasticPrograms.MultipleKnownSet
                continue
            elseif F <: VariableRef || F <: DecisionRef
                continue
            elseif F <: AffExpr
                i += 1
                f = MOI.get(model, MOI.ConstraintFunction(), cref)
                for term in f.terms
                    idx = term.variable_index.value - nx
                    if idx == fractional_index
                        # yⱼ
                        push!(Hᵢ, i)
                        push!(Hᵥ, term.coefficient)
                        push!(Hⱼ, idx)
                    else
                        # yⱼ
                        push!(Fᵢ, i)
                        push!(Fᵥ, term.coefficient)
                        push!(Fⱼ, nx + ny + nx + idx)
                        # 1-yⱼ
                        push!(Hᵢ, i+1)
                        push!(Hᵥ, term.coefficient)
                        push!(Hⱼ, idx)
                        push!(Fᵢ, i+1)
                        push!(Fᵥ, -term.coefficient)
                        push!(Fⱼ, nx + ny + nx + idx)
                    end
                end
                set = MOI.get(model, MOI.ConstraintSet(), cref)
                if set isa MOI.LessThan
                    # yⱼ
                    push!(lhs, -Inf)
                    push!(rhs, zero(T))
                    push!(Hᵢ, i)
                    push!(Hᵥ, -set.upper)
                    push!(Hⱼ, fractional_index)
                    # 1-yⱼ
                    push!(lhs, -Inf)
                    push!(rhs, set.upper)
                    push!(Hᵢ, i+1)
                    push!(Hᵥ, set.upper)
                    push!(Hⱼ, fractional_index)
                elseif set isa MOI.GreaterThan
                    # yⱼ
                    push!(lhs, zero(T))
                    push!(rhs, Inf)
                    push!(Hᵢ, i)
                    push!(Hᵥ, -set.lower)
                    push!(Hⱼ, fractional_index)
                    # 1-yⱼ
                    push!(lhs, set.lower)
                    push!(rhs, Inf)
                    push!(Hᵢ, i+1)
                    push!(Hᵥ, set.lower)
                    push!(Hⱼ, fractional_index)
                elseif set isa MOI.EqualTo
                    # yⱼ
                    push!(lhs, zero(T))
                    push!(rhs, zero(T))
                    push!(Hᵢ, i)
                    push!(Hᵥ, -set.value)
                    push!(Hⱼ, fractional_index)
                    # 1-yⱼ
                    push!(lhs, set.value)
                    push!(rhs, set.value)
                    push!(Hᵢ, i+1)
                    push!(Hᵥ, set.value)
                    push!(Hⱼ, fractional_index)
                end
                i += 1
            elseif F <: DecisionAffExpr
                i += 1
                f = MOI.get(model, MOI.ConstraintFunction(), cref)
                for term in f.variable_part.terms
                    idx = term.variable_index.value - nx
                    if idx == fractional_index
                        # yⱼ
                        push!(Hᵢ, i)
                        push!(Hᵥ, term.coefficient)
                        push!(Hⱼ, idx)
                    else
                        # yⱼ
                        push!(Fᵢ, i)
                        push!(Fᵥ, term.coefficient)
                        push!(Fⱼ, nx + idx - 1)
                        # 1-yⱼ
                        push!(Hᵢ, i+1)
                        push!(Hᵥ, term.coefficient)
                        push!(Hⱼ, idx)
                        push!(Fᵢ, i+1)
                        push!(Fᵥ, -term.coefficient)
                        push!(Fⱼ, nx + idx - 1)
                    end
                end
                for term in f.decision_part.terms
                    idx = term.variable_index.value - nx
                    if idx == fractional_index
                        # yⱼ
                        push!(Hᵢ, i)
                        push!(Hᵥ, term.coefficient)
                        push!(Hⱼ, idx)
                    else
                        # yⱼ
                        push!(Fᵢ, i)
                        push!(Fᵥ, term.coefficient)
                        push!(Fⱼ, nx + idx - 1)
                        # 1-yⱼ
                        push!(Hᵢ, i+1)
                        push!(Hᵥ, term.coefficient)
                        push!(Hⱼ, idx)
                        push!(Fᵢ, i+1)
                        push!(Fᵥ, -term.coefficient)
                        push!(Fⱼ, nx + idx - 1)
                    end
                end
                for term in f.known_part.terms
                    idx = term.variable_index.value
                    # yⱼ
                    push!(Fᵢ, i)
                    push!(Fᵥ, term.coefficient)
                    push!(Fⱼ, idx)
                    # 1-yⱼ
                    push!(Gᵢ, i+1)
                    push!(Gᵥ, term.coefficient)
                    push!(Gⱼ, idx)
                    push!(Fᵢ, i+1)
                    push!(Fᵥ, -term.coefficient)
                    push!(Fⱼ, idx)
                end
                set = MOI.get(model, MOI.ConstraintSet(), cref)
                if set isa MOI.LessThan
                    # yⱼ
                    push!(lhs, -Inf)
                    push!(rhs, zero(T))
                    push!(Hᵢ, i)
                    push!(Hᵥ, -set.upper)
                    push!(Hⱼ, fractional_index)
                    # 1-yⱼ
                    push!(lhs, -Inf)
                    push!(rhs, set.upper)
                    push!(Hᵢ, i+1)
                    push!(Hᵥ, set.upper)
                    push!(Hⱼ, fractional_index)
                elseif set isa MOI.GreaterThan
                    # yⱼ
                    push!(lhs, zero(T))
                    push!(rhs, Inf)
                    push!(Hᵢ, i)
                    push!(Hᵥ, -set.lower)
                    push!(Hⱼ, fractional_index)
                    # 1-yⱼ
                    push!(lhs, set.lower)
                    push!(rhs, Inf)
                    push!(Hᵢ, i+1)
                    push!(Hᵥ, set.lower)
                    push!(Hⱼ, fractional_index)
                elseif set isa MOI.EqualTo
                    # yⱼ
                    push!(lhs, zero(T))
                    push!(rhs, zero(T))
                    push!(Hᵢ, i)
                    push!(Hᵥ, -set.value)
                    push!(Hⱼ, fractional_index)
                    # 1-yⱼ
                    push!(lhs, set.value)
                    push!(rhs, set.value)
                    push!(Hᵢ, i+1)
                    push!(Hᵥ, set.value)
                    push!(Hⱼ, fractional_index)
                end
                i += 1
            else
                error("Cannot extract linear representation from $F-$S constraint.")
            end
        end
    end
    # x ≥ 0
    for j in 1:length(x̄)
        i += 1
        # yⱼ
        push!(Fᵢ, i)
        push!(Fᵥ, 1.)
        push!(Fⱼ, j)
        push!(lhs, zero(T))
        push!(rhs, Inf)
        # 1-yⱼ
        push!(Gᵢ, i+1)
        push!(Gᵥ, 1.)
        push!(Gⱼ, j)
        push!(Fᵢ, i+1)
        push!(Fᵥ, -1.)
        push!(Fⱼ, j)
        push!(lhs, zero(T))
        push!(rhs, Inf)
        i += 1
    end
    # x ≤ 1
    for j in 1:length(x̄)
        i += 1
        # yⱼ
        push!(Fᵢ, i)
        push!(Fᵥ, 1.)
        push!(Fⱼ, j)
        push!(Hᵢ, i)
        push!(Hᵥ, -1.)
        push!(Hⱼ, fractional_index)
        push!(lhs, -Inf)
        push!(rhs, zero(T))
        # 1-yⱼ
        push!(Gᵢ, i+1)
        push!(Gᵥ, 1.)
        push!(Gⱼ, j)
        push!(Fᵢ, i+1)
        push!(Fᵥ, -1.)
        push!(Fⱼ, j)
        push!(Hᵢ, i+1)
        push!(Hᵥ, 1.)
        push!(Hⱼ, fractional_index)
        push!(lhs, -Inf)
        push!(rhs, 1.)
        i += 1
    end
    # y ≥ 0
    for j in 1:length(ȳ)
        i += 1
        if j == fractional_index
            # yⱼ
            push!(Hᵢ, i)
            push!(Hᵥ, 1.)
            push!(Hⱼ, j)
            push!(lhs, zero(T))
            push!(rhs, Inf)
        else
            # yⱼ
            push!(Fᵢ, i)
            push!(Fᵥ, 1.)
            push!(Fⱼ, nx + j - 1)
            push!(lhs, zero(T))
            push!(rhs, Inf)
            # 1-yⱼ
            push!(Hᵢ, i+1)
            push!(Hᵥ, 1.)
            push!(Hⱼ, j)
            push!(Fᵢ, i+1)
            push!(Fᵥ, -1.)
            push!(Fⱼ, nx + j - 1)
            push!(lhs, zero(T))
            push!(rhs, Inf)
            i += 1
        end
    end
    # y ≤ 1
    for j in 1:length(ȳ)
        i += 1
        if j == fractional_index
            # 1-yⱼ
            push!(Hᵢ, i)
            push!(Hᵥ, 1.)
            push!(Hⱼ, j)
            push!(lhs, -Inf)
            push!(rhs, 1.)
        else
            # yⱼ
            push!(Fᵢ, i)
            push!(Fᵥ, 1.)
            push!(Fⱼ, nx + j - 1)
            push!(Hᵢ, i)
            push!(Hᵥ, -1.)
            push!(Hⱼ, fractional_index)
            push!(lhs, -Inf)
            push!(rhs, zero(T))
            # 1-yⱼ
            push!(Hᵢ, i+1)
            push!(Hᵥ, 1.)
            push!(Hⱼ, j)
            push!(Fᵢ, i+1)
            push!(Fᵥ, -1.)
            push!(Fⱼ, nx + j - 1)
            push!(Hᵢ, i+1)
            push!(Hᵥ, 1.)
            push!(Hⱼ, fractional_index)
            push!(lhs, -Inf)
            push!(rhs, 1.)
            i += 1
        end
    end
    G̃ = sparse(Gᵢ, Gⱼ, Gᵥ, i, nx)
    H̃ = sparse(Hᵢ, Hⱼ, Hᵥ, i, ny)
    F̃ = sparse(Fᵢ, Fⱼ, Fᵥ, i, nw)
    G, f = StochasticPrograms.canonical(G̃, lhs, rhs; direction = :geq)
    H, f = StochasticPrograms.canonical(H̃, lhs, rhs; direction = :geq)
    F, f = StochasticPrograms.canonical(F̃, lhs, rhs; direction = :geq)
    # Generate CGLP
    CGLP = Model(worker.parameters.optimizer)
    n = size(F, 1)
    @variable(CGLP, π[1:n] >= 0)
    @objective(CGLP, Min, π⋅(H*ȳ) - π⋅(f - G*x̄))
    @constraint(CGLP, F'*π .== 0)
    @constraint(CGLP, sum(π) == 1)
    # Solve CGLP
    optimize!(CGLP)
    if termination_status(CGLP) == MOI.OPTIMAL
        π = value.(π)
        πx = G'*π
        πy = H'*π
        g = π⋅f
        # Extend standard form LP with new cuts
        add_row!(worker.data.relaxation, πx, πy, g)
        # Return cutting plane
        return πx, πy, g
    else
        error("CGLP could not be solved.")
    end
end

function cutting_plane(worker::ParametricGomoryWorker,
                       ::CuttingPlaneTree,
                       subproblem::SubProblem,
                       x̄::AbstractVector,
                       ȳ::AbstractVector,
                       fractional::Vector{MOI.VariableIndex})
    # Check optimizer
    if worker.parameters.optimizer === nothing
        error("Cannot generate CGLP without optimizer.")
    end
    # Largest fractional index
    fractional_index = fractional[end].value - length(x̄)
    # Dimensions
    lp = worker.data.relaxation
    m  = size(lp.T, 1)
    nx = size(lp.T, 2)
    ny = size(lp.W, 2)
    length(x̄) == nx || error("Given first-stage decision $x̄ does not match second-stage problem dimensions.")
    length(ȳ) == ny || error("Given second-stage decision $ȳ does not match second-stage problem dimensions.")
    ȳ[abs.(ȳ) .<= sqrt(eps())] .= 0.0
    x_lb = fill(0., nx)
    x_ub = fill(1., nx)
    lb_finite = isfinite.(lp.lb)
    ub_finite = isfinite.(lp.ub)
    # Generate CGLP
    CGLP = Model(worker.parameters.optimizer)
    @variable(CGLP, πx[1:nx])
    @variable(CGLP, πy[1:ny])
    @variable(CGLP, tx >= 0)
    @variable(CGLP, ty >= 0)
    @variable(CGLP, λ₂₁[1:m] >= 0)
    @variable(CGLP, λ₂₂[1:m] >= 0)
    @variable(CGLP, ν₁₁[1:nx] >= 0)
    @variable(CGLP, ν₁₂[1:nx] >= 0)
    @variable(CGLP, ν₂₁[1:ny] >= 0)
    @variable(CGLP, ν₂₂[1:ny] >= 0)
    @variable(CGLP, μ₁₁[1:nx] >= 0)
    @variable(CGLP, μ₁₂[1:nx] >= 0)
    @variable(CGLP, μ₂₁[1:ny] >= 0)
    @variable(CGLP, μ₂₂[1:ny] >= 0)
    @objective(CGLP, Min, tx + ty)
    @constraint(CGLP, vcat(tx, πx) in MOI.NormOneCone(nx+1))
    @constraint(CGLP, vcat(ty, πy) in MOI.NormOneCone(ny+1))
    @constraint(CGLP, πx .== lp.T'*λ₂₁ + μ₁₁ - ν₁₁)
    @constraint(CGLP, πx .== lp.T'*λ₂₂ + μ₁₂ - ν₁₂)
    @constraint(CGLP, πy .== lp.W'*λ₂₁ + μ₂₁ - ν₂₁)
    @constraint(CGLP, πy .== lp.W'*λ₂₂ + μ₂₂ - ν₂₂)
    lp.ub[fractional_index] = 0.
    @constraint(CGLP, lp.h⋅λ₂₁ + x_lb⋅μ₁₁ + lp.lb[lb_finite]⋅μ₂₁[lb_finite] - x_ub⋅ν₁₁ - lp.ub[ub_finite]⋅ν₂₁[ub_finite] >= πx⋅x̄ + πy⋅ȳ + 1)
    lp.ub[fractional_index] = 1.
    lp.lb[fractional_index] = 1.
    @constraint(CGLP, lp.h⋅λ₂₂ + x_lb⋅μ₁₂ + lp.lb[lb_finite]⋅μ₂₂[lb_finite] - x_ub⋅ν₁₂ - lp.ub[ub_finite]⋅ν₂₂[ub_finite] >= πx⋅x̄ + πy⋅ȳ + 1)
    lp.lb[fractional_index] = 0.
    # Solve CGLP
    optimize!(CGLP)
    if termination_status(CGLP) == MOI.OPTIMAL
        # Extract cutting plane parameters
        πx = value.(CGLP[:πx])
        πy = value.(CGLP[:πy])
        g  = πx⋅x̄ + πy⋅ȳ + 1.
        # Extend standard form LP with new cuts
        add_row!(worker.data.relaxation, πx, πy, g)
        # Return cutting plane
        return πx, πy, g
    else
        error("CGLP could not be solved to optimality.")
    end
end

# API
# ------------------------------------------------------------
"""
    ParametricGomory

Factory object for using convexification to handle integer recourse. Pass to `integer_strategy` in `LShaped.Optimizer` or set the [`IntegerStrategy`](@ref) attribute.

"""
struct ParametricGomory <: AbstractIntegerStrategy
    parameters::ParametricGomoryParameters
end
ParametricGomory(; kw...) = ParametricGomory(ParametricGomoryParameters(; kw...))

function master(wc::ParametricGomory, ::Type{T}) where T <: AbstractFloat
    return ParametricGomoryMaster()
end

function worker(wc::ParametricGomory, ::Type{T}) where T <: AbstractFloat
    return ParametricGomoryWorker(T; type2dict(wc.parameters)...)
end
function worker_type(::ParametricGomory)
    return ParametricGomoryWorker{Float64}
end


typealias Scene Frame{Vehicle}
Scene(n::Int=100) = Frame(Vehicle, n)
Scene(arr::Vector{Vehicle}) = Frame{Vehicle}(arr, length(arr))

Base.show(io, scene::Scene) = print(io, "Scene(with $(length(scene)) cars)")

"""
    get_neighbor_index_fore(scene::Scene, vehicle_index::Int, roadway::Roadway)
Return the index of the vehicle that is in the same lane as scene[vehicle_index] and
in front of it with the smallest distance along the lane

    The method will search on the current lane first, and if no vehicle is found it
    will continue to travel along the lane following next_lane(lane, roadway).
    If no vehicle is found within `max_distance_fore,` a value of 0 is returned instead.
"""
immutable NeighborLongitudinalResult
    ind::Int # index in scene of the neighbor
    Δs::Float64 # positive distance along lane between vehicles' positions
end

export
    VehicleTargetPoint,
    VehicleTargetPointFront,
    VehicleTargetPointCenter,
    VehicleTargetPointRear,
    get_targetpoint_delta

abstract VehicleTargetPoint
immutable VehicleTargetPointFront <: VehicleTargetPoint end
get_targetpoint_delta(::VehicleTargetPointFront, veh::Vehicle) = veh.def.length/2*cos(veh.state.posF.ϕ)
immutable VehicleTargetPointCenter <: VehicleTargetPoint end
get_targetpoint_delta(::VehicleTargetPointCenter, veh::Vehicle) = 0.0
immutable VehicleTargetPointRear <: VehicleTargetPoint end
get_targetpoint_delta(::VehicleTargetPointRear, veh::Vehicle) = -veh.def.length/2*cos(veh.state.posF.ϕ)

const VEHICLE_TARGET_POINT_CENTER = VehicleTargetPointCenter()

function get_neighbor_fore_along_lane(
    scene::Scene,
    roadway::Roadway,
    tag_start::LaneTag,
    s_base::Float64,
    targetpoint_primary::VehicleTargetPoint, # the reference point whose distance we want to minimize
    targetpoint_valid::VehicleTargetPoint; # the reference point, which if distance to is positive, we include the vehicle
    max_distance_fore::Float64 = 250.0, # max distance to search forward [m]
    index_to_ignore::Int=-1,
    )

    best_ind = 0
    best_dist = max_distance_fore
    tag_target = tag_start

    dist_searched = 0.0
    while dist_searched < max_distance_fore

        lane = roadway[tag_target]

        for (i,veh) in enumerate(scene)
            if i != index_to_ignore

                s_adjust = NaN
                if veh.state.posF.roadind.tag == tag_target
                    s_adjust = 0.0

                elseif is_between_segments_hi(veh.state.posF.roadind.ind, lane.curve) &&
                       is_in_entrances(roadway[tag_target], veh.state.posF.roadind.tag)

                    distance_between_lanes = abs(roadway[tag_target].curve[1].pos - roadway[veh.state.posF.roadind.tag].curve[end].pos)
                    s_adjust = -(roadway[veh.state.posF.roadind.tag].curve[end].s + distance_between_lanes)

                elseif is_between_segments_lo(veh.state.posF.roadind.ind) &&
                       is_in_exits(roadway[tag_target], veh.state.posF.roadind.tag)

                    distance_between_lanes = abs(roadway[tag_target].curve[end].pos - roadway[veh.state.posF.roadind.tag].curve[1].pos)
                    s_adjust = roadway[tag_target].curve[end].s + distance_between_lanes
                end

                if !isnan(s_adjust)
                    s_valid = veh.state.posF.s + get_targetpoint_delta(targetpoint_valid, veh) + s_adjust
                    dist_valid = s_valid - s_base + dist_searched
                    if dist_valid ≥ 0.0
                        s_primary = veh.state.posF.s + get_targetpoint_delta(targetpoint_primary, veh) + s_adjust
                        dist = s_primary - s_base + dist_searched
                        if dist < best_dist
                            best_dist = dist
                            best_ind = i
                        end
                    end
                end
            end
        end

        if best_ind != 0
            break
        end

        if !has_next(lane) ||
           (tag_target == tag_start && dist_searched != 0.0) # exit after visiting this lane a 2nd time
            break
        end

        dist_searched += (lane.curve[end].s - s_base)
        s_base = -abs(lane.curve[end].pos - next_lane_point(lane, roadway).pos) # negative distance between lanes
        tag_target = next_lane(lane, roadway).tag
    end

    NeighborLongitudinalResult(best_ind, best_dist)
end
function get_neighbor_fore_along_lane(
    scene::Scene,
    vehicle_index::Int,
    roadway::Roadway,
    targetpoint_ego::VehicleTargetPoint,
    targetpoint_primary::VehicleTargetPoint, # the reference point whose distance we want to minimize
    targetpoint_valid::VehicleTargetPoint; # the reference point, which if distance to is positive, we include the vehicle
    max_distance_fore::Float64 = 250.0 # max distance to search forward [m]
    )

    veh_ego = scene[vehicle_index]
    tag_start = veh_ego.state.posF.roadind.tag
    s_base = veh_ego.state.posF.s + get_targetpoint_delta(targetpoint_ego, veh_ego)

    get_neighbor_fore_along_lane(scene, roadway, tag_start, s_base,
        targetpoint_primary, targetpoint_valid,
        max_distance_fore=max_distance_fore, index_to_ignore=vehicle_index)
end
function get_neighbor_fore_along_left_lane(
    scene::Scene,
    vehicle_index::Int,
    roadway::Roadway,
    targetpoint_ego::VehicleTargetPoint,
    targetpoint_primary::VehicleTargetPoint, # the reference point whose distance we want to minimize
    targetpoint_valid::VehicleTargetPoint; # the reference point, which if distance to is positive, we include the vehicle
    max_distance_fore::Float64 = 250.0 # max distance to search forward [m]
    )

    retval = NeighborLongitudinalResult(0, max_distance_fore)

    veh_ego = scene[vehicle_index]
    lane = roadway[veh_ego.state.posF.roadind.tag]
    if n_lanes_left(lane, roadway) > 0
        lane_left = roadway[LaneTag(lane.tag.segment, lane.tag.lane + 1)]
        roadproj = proj(veh_ego.state.posG, lane_left, roadway)
        tag_start = roadproj.tag
        s_base = lane_left[roadproj.curveproj.ind, roadway].s + get_targetpoint_delta(targetpoint_ego, veh_ego)

        retval = get_neighbor_fore_along_lane(scene, roadway, tag_start, s_base,
                                              targetpoint_primary, targetpoint_valid,
                                              index_to_ignore=vehicle_index,
                                              max_distance_fore=max_distance_fore)
    end

    retval
end
function get_neighbor_fore_along_right_lane(
    scene::Scene,
    vehicle_index::Int,
    roadway::Roadway,
    targetpoint_ego::VehicleTargetPoint,
    targetpoint_primary::VehicleTargetPoint, # the reference point whose distance we want to minimize
    targetpoint_valid::VehicleTargetPoint; # the reference point, which if distance to is positive, we include the vehicle
    max_distance_fore::Float64 = 250.0 # max distance to search forward [m]
    )

    retval = NeighborLongitudinalResult(0, max_distance_fore)

    veh_ego = scene[vehicle_index]
    lane = roadway[veh_ego.state.posF.roadind.tag]
    if n_lanes_right(lane, roadway) > 0
        lane_right = roadway[LaneTag(lane.tag.segment, lane.tag.lane - 1)]
        roadproj = proj(veh_ego.state.posG, lane_right, roadway)
        tag_start = roadproj.tag
        s_base = lane_right[roadproj.curveproj.ind, roadway].s + get_targetpoint_delta(targetpoint_ego, veh_ego)

        retval = get_neighbor_fore_along_lane(scene, roadway, tag_start, s_base,
                                              targetpoint_primary, targetpoint_valid,
                                              index_to_ignore=vehicle_index,
                                              max_distance_fore=max_distance_fore)
    end

    retval
end

function get_neighbor_fore_along_lane(
    scene::Scene,
    roadway::Roadway,
    tag_start::LaneTag,
    s_base::Float64;
    max_distance_fore::Float64 = 250.0, # max distance to search forward [m]
    index_to_ignore::Int=-1,
    )

    get_neighbor_fore_along_lane(scene, roadway, tag_start, s_base,
        VEHICLE_TARGET_POINT_CENTER, VEHICLE_TARGET_POINT_CENTER,
        max_distance_fore=max_distance_fore,
        index_to_ignore=index_to_ignore)
end
function get_neighbor_fore_along_lane(scene::Scene, vehicle_index::Int, roadway::Roadway;
    max_distance_fore::Float64 = 250.0 # max distance to search forward [m]
    )

    get_neighbor_fore_along_lane(scene, vehicle_index, roadway,
        VEHICLE_TARGET_POINT_CENTER, VEHICLE_TARGET_POINT_CENTER,
        VEHICLE_TARGET_POINT_CENTER, max_distance_fore=max_distance_fore)
end
function get_neighbor_fore_along_left_lane(scene::Scene, vehicle_index::Int, roadway::Roadway;
    max_distance_fore::Float64 = 250.0 # max distance to search forward [m]
    )

    get_neighbor_fore_along_left_lane(scene, vehicle_index, roadway,
        VEHICLE_TARGET_POINT_CENTER, VEHICLE_TARGET_POINT_CENTER,
        VEHICLE_TARGET_POINT_CENTER, max_distance_fore=max_distance_fore)
end
function get_neighbor_fore_along_right_lane(scene::Scene, vehicle_index::Int, roadway::Roadway;
    max_distance_fore::Float64 = 250.0 # max distance to search forward [m]
    )

    get_neighbor_fore_along_right_lane(scene, vehicle_index, roadway,
        VEHICLE_TARGET_POINT_CENTER, VEHICLE_TARGET_POINT_CENTER,
        VEHICLE_TARGET_POINT_CENTER, max_distance_fore=max_distance_fore)
end

function get_neighbor_rear_along_lane(
    scene::Scene,
    roadway::Roadway,
    tag_start::LaneTag,
    s_base::Float64,
    targetpoint_primary::VehicleTargetPoint, # the reference point whose distance we want to minimize
    targetpoint_valid::VehicleTargetPoint; # the reference point, which if distance to is positive, we include the vehicle
    max_distance_rear::Float64 = 250.0, # max distance to search rearward [m]
    index_to_ignore::Int=-1,
    )

    best_ind = 0
    best_dist = max_distance_rear
    tag_target = tag_start

    ignore = Set{Int}()

    dist_searched = 0.0
    while dist_searched < max_distance_rear

        lane = roadway[tag_target]

        for (i,veh) in enumerate(scene)
            if i != index_to_ignore && !in(veh.id, ignore)

                s_adjust = NaN

                if veh.state.posF.roadind.tag == tag_target
                    s_adjust = 0.0

                elseif is_between_segments_hi(veh.state.posF.roadind.ind, lane.curve) &&
                       is_in_entrances(roadway[tag_target], veh.state.posF.roadind.tag)

                    distance_between_lanes = abs(roadway[tag_target].curve[1].pos - roadway[veh.state.posF.roadind.tag].curve[end].pos)
                    s_adjust = -(roadway[veh.state.posF.roadind.tag].curve[end].s + distance_between_lanes)

                elseif is_between_segments_lo(veh.state.posF.roadind.ind) &&
                       is_in_exits(roadway[tag_target], veh.state.posF.roadind.tag)

                    distance_between_lanes = abs(roadway[tag_target].curve[end].pos - roadway[veh.state.posF.roadind.tag].curve[1].pos)
                    s_adjust = roadway[tag_target].curve[end].s + distance_between_lanes
                end

                if !isnan(s_adjust)
                    s_valid = veh.state.posF.s + get_targetpoint_delta(targetpoint_valid, veh) + s_adjust
                    dist_valid = s_base - s_valid + dist_searched
                    if dist_valid ≥ 0.0
                        s_primary = veh.state.posF.s + get_targetpoint_delta(targetpoint_primary, veh) + s_adjust
                        dist = s_base - s_primary + dist_searched
                        if dist < best_dist
                            best_dist = dist
                            best_ind = i
                        end
                    else
                        push!(ignore, veh.id)
                    end
                end
            end
        end

        if best_ind != 0
            break
        end

        if !has_prev(lane) ||
           (tag_target == tag_start && dist_searched != 0.0) # exit after visiting this lane a 2nd time
            break
        end

        dist_searched += s_base
        s_base = lane.curve[end].s + abs(lane.curve[end].pos - prev_lane_point(lane, roadway).pos) # end of prev lane plus crossover
        tag_target = prev_lane(lane, roadway).tag
    end

    NeighborLongitudinalResult(best_ind, best_dist)
end
function get_neighbor_rear_along_lane(
    scene::Scene,
    vehicle_index::Int,
    roadway::Roadway,
    targetpoint_ego::VehicleTargetPoint,
    targetpoint_primary::VehicleTargetPoint, # the reference point whose distance we want to minimize
    targetpoint_valid::VehicleTargetPoint; # the reference point, which if distance to is positive, we include the vehicle
    max_distance_rear::Float64 = 250.0 # max distance to search rearward [m]
    )

    veh_ego = scene[vehicle_index]
    tag_start = veh_ego.state.posF.roadind.tag
    s_base = veh_ego.state.posF.s + get_targetpoint_delta(targetpoint_ego, veh_ego)

    get_neighbor_rear_along_lane(scene, roadway, tag_start, s_base,
        targetpoint_primary, targetpoint_valid,
        max_distance_rear=max_distance_rear, index_to_ignore=vehicle_index)
end
function get_neighbor_rear_along_left_lane(
    scene::Scene,
    vehicle_index::Int,
    roadway::Roadway,
    targetpoint_ego::VehicleTargetPoint,
    targetpoint_primary::VehicleTargetPoint, # the reference point whose distance we want to minimize
    targetpoint_valid::VehicleTargetPoint; # the reference point, which if distance to is positive, we include the vehicle
    max_distance_rear::Float64 = 250.0 # max distance to search forward [m]
    )

    retval = NeighborLongitudinalResult(0, max_distance_rear)

    veh_ego = scene[vehicle_index]
    lane = roadway[veh_ego.state.posF.roadind.tag]
    if n_lanes_left(lane, roadway) > 0
        lane_left = roadway[LaneTag(lane.tag.segment, lane.tag.lane + 1)]
        roadproj = proj(veh_ego.state.posG, lane_left, roadway)
        tag_start = roadproj.tag
        s_base = lane_left[roadproj.curveproj.ind, roadway].s + get_targetpoint_delta(targetpoint_ego, veh_ego)

        retval = get_neighbor_rear_along_lane(scene, roadway, tag_start, s_base,
                                              targetpoint_primary, targetpoint_valid,
                                              index_to_ignore=vehicle_index,
                                              max_distance_rear=max_distance_rear)
    end

    retval
end
function get_neighbor_rear_along_right_lane(
    scene::Scene,
    vehicle_index::Int,
    roadway::Roadway,
    targetpoint_ego::VehicleTargetPoint,
    targetpoint_primary::VehicleTargetPoint, # the reference point whose distance we want to minimize
    targetpoint_valid::VehicleTargetPoint; # the reference point, which if distance to is positive, we include the vehicle
    max_distance_rear::Float64 = 250.0 # max distance to search forward [m]
    )

    retval = NeighborLongitudinalResult(0, max_distance_rear)

    veh_ego = scene[vehicle_index]
    lane = roadway[veh_ego.state.posF.roadind.tag]
    if n_lanes_right(lane, roadway) > 0
        lane_right = roadway[LaneTag(lane.tag.segment, lane.tag.lane - 1)]
        roadproj = proj(veh_ego.state.posG, lane_right, roadway)
        tag_start = roadproj.tag
        s_base = lane_right[roadproj.curveproj.ind, roadway].s + get_targetpoint_delta(targetpoint_ego, veh_ego)

        retval = get_neighbor_rear_along_lane(scene, roadway, tag_start, s_base,
                                              targetpoint_primary, targetpoint_valid,
                                              index_to_ignore=vehicle_index,
                                              max_distance_rear=max_distance_rear)
    end

    retval
end

function get_neighbor_rear_along_lane(
    scene::Scene,
    roadway::Roadway,
    tag_start::LaneTag,
    s_base::Float64;
    max_distance_rear::Float64 = 250.0, # max distance to search rearward [m]
    index_to_ignore::Int=-1,
    )

    get_neighbor_rear_along_lane(scene, roadway, tag_start, s_base,
        VEHICLE_TARGET_POINT_CENTER, VEHICLE_TARGET_POINT_CENTER,
        max_distance_rear=max_distance_rear,
        index_to_ignore=index_to_ignore)
end
function get_neighbor_rear_along_lane(scene::Scene, vehicle_index::Int, roadway::Roadway;
    max_distance_rear::Float64 = 250.0 # max distance to search forward [m]
    )

    get_neighbor_rear_along_lane(scene, vehicle_index, roadway,
        VEHICLE_TARGET_POINT_CENTER, VEHICLE_TARGET_POINT_CENTER,
        VEHICLE_TARGET_POINT_CENTER, max_distance_rear=max_distance_rear)
end
function get_neighbor_rear_along_left_lane(
    scene::Scene,
    vehicle_index::Int,
    roadway::Roadway;
    max_distance_rear::Float64 = 250.0 # max distance to search forward [m]
    )

    get_neighbor_rear_along_left_lane(scene, vehicle_index, roadway,
        VEHICLE_TARGET_POINT_CENTER, VEHICLE_TARGET_POINT_CENTER,
        VEHICLE_TARGET_POINT_CENTER, max_distance_rear=max_distance_rear)
end
function get_neighbor_rear_along_right_lane(
    scene::Scene,
    vehicle_index::Int,
    roadway::Roadway;
    max_distance_rear::Float64 = 250.0 # max distance to search forward [m]
    )

    get_neighbor_rear_along_right_lane(scene, vehicle_index, roadway,
        VEHICLE_TARGET_POINT_CENTER, VEHICLE_TARGET_POINT_CENTER,
        VEHICLE_TARGET_POINT_CENTER, max_distance_rear=max_distance_rear)
end

"""
    Project the given point to the same lane as the given RoadIndex.
This will return the projection of the point, along with the Δs along the lane from the RoadIndex.
"""
immutable FrenetRelativePosition
    origin::RoadIndex
    target::RoadIndex
    Δs::Float64
    t::Float64
    ϕ::Float64
end
function get_frenet_relative_position(posG::VecSE2, roadind::RoadIndex, roadway::Roadway;
    max_distance_fore::Float64 = 250.0, # max distance to search forward [m]
    max_distance_rear::Float64 = 250.0, # max distance to search backward [m]
    improvement_threshold::Float64 = 1e-4,
    )

    # project to current lane first
    tag_start = roadind.tag
    lane_start = roadway[tag_start]
    curveproj_start = proj(posG, lane_start, roadway, move_along_curves=false).curveproj
    curvept_start = lane_start[curveproj_start.ind, roadway]
    s_base = lane_start[roadind.ind, roadway].s
    s_proj = curvept_start.s
    Δs = s_proj - s_base
    sq_dist_to_curve = abs2(posG - curvept_start.pos)
    retval = FrenetRelativePosition(roadind,
                RoadIndex(curveproj_start.ind, tag_start), Δs, curveproj_start.t, curveproj_start.ϕ)

    # search downstream
    if has_next(lane_start)
        dist_searched = lane_start.curve[end].s - s_base
        s_base = -abs(lane_start.curve[end].pos - next_lane_point(lane_start, roadway).pos) # negative distance between lanes
        tag_target = next_lane(lane_start, roadway).tag
        while dist_searched < max_distance_fore

            lane = roadway[tag_target]
            curveproj = proj(posG, lane, roadway, move_along_curves=false).curveproj
            sq_dist_to_curve2 = abs2(posG - lane[curveproj.ind, roadway].pos)

            if sq_dist_to_curve2 < sq_dist_to_curve - improvement_threshold

                sq_dist_to_curve = sq_dist_to_curve2
                s_proj = lane[curveproj.ind, roadway].s
                Δs = s_proj - s_base + dist_searched
                retval = FrenetRelativePosition(
                            roadind, RoadIndex(curveproj.ind, tag_target),
                            Δs, curveproj.t, curveproj.ϕ)
            end

            if !has_next(lane)
                break
            end

            dist_searched += (lane.curve[end].s - s_base)
            s_base = -abs(lane.curve[end].pos - next_lane_point(lane, roadway).pos) # negative distance between lanes
            tag_target = next_lane(lane, roadway).tag

            if tag_target == tag_start
                break
            end
        end
    end

    # search upstream
    if has_prev(lane_start)
        dist_searched = s_base
        tag_target = prev_lane(lane_start, roadway).tag
        s_base = roadway[tag_target].curve[end].s + abs(lane_start.curve[1].pos - prev_lane_point(lane_start, roadway).pos) # end of the lane

        while dist_searched < max_distance_fore

            lane = roadway[tag_target]
            curveproj = proj(posG, lane, roadway, move_along_curves=false).curveproj
            sq_dist_to_curve2 = abs2(posG - lane[curveproj.ind, roadway].pos)

            if sq_dist_to_curve2 < sq_dist_to_curve - improvement_threshold

                sq_dist_to_curve = sq_dist_to_curve2
                s_proj = lane[curveproj.ind, roadway].s
                dist = s_base - s_proj  + dist_searched
                Δs = -dist
                retval = FrenetRelativePosition(
                            roadind, RoadIndex(curveproj.ind, tag_target),
                            Δs, curveproj.t, curveproj.ϕ)
            end

            if !has_prev(lane)
                break
            end

            dist_searched += s_base
            s_base = lane.curve[end].s + abs(lane.curve[1].pos - prev_lane_point(lane, roadway).pos) # length of this lane plus crossover
            tag_target = prev_lane(lane, roadway).tag

            if tag_target == tag_start
                break
            end
        end
    end

    retval
end
get_frenet_relative_position(veh_fore::Vehicle, veh_rear::Vehicle, roadway::Roadway) = get_frenet_relative_position(veh_fore.state.posG, veh_rear.state.posF.roadind, roadway)


export iauAtciqn
"""
Quick ICRS, epoch J2000.0, to CIRS transformation, given precomputed
star-independent astrometry parameters plus a list of light-
deflecting bodies.

Use of this function is appropriate when efficiency is important and
where many star positions are to be transformed for one date.  The
star-independent parameters can be obtained by calling one of the
functions iauApci[13], iauApcg[13], iauApco[13] or iauApcs[13].

If the only light-deflecting body to be taken into account is the
Sun, the iauAtciq function can be used instead.  If in addition the
parallax and proper motions are zero, the iauAtciqz function can be
used.

This function is part of the International Astronomical Union's
SOFA (Standards of Fundamental Astronomy) software collection.

Status:  support function.

Given:
   rc,dc  double       ICRS RA,Dec at J2000.0 (radians)
   pr     double       RA proper motion (radians/year; Note 3)
   pd     double       Dec proper motion (radians/year)
   px     double       parallax (arcsec)
   rv     double       radial velocity (km/s, +ve if receding)
   astrom iauASTROM*   star-independent astrometry parameters:
      pmt    double       PM time interval (SSB, Julian years)
      eb     double[3]    SSB to observer (vector, au)
      eh     double[3]    Sun to observer (unit vector)
      em     double       distance from Sun to observer (au)
      v      double[3]    barycentric observer velocity (vector, c)
      bm1    double       sqrt(1-|v|^2): reciprocal of Lorenz factor
      bpn    double[3][3] bias-precession-nutation matrix
      along  double       longitude + s' (radians)
      xpl    double       polar motion xp wrt local meridian (radians)
      ypl    double       polar motion yp wrt local meridian (radians)
      sphi   double       sine of geodetic latitude
      cphi   double       cosine of geodetic latitude
      diurab double       magnitude of diurnal aberration vector
      eral   double       "local" Earth rotation angle (radians)
      refa   double       refraction constant A (radians)
      refb   double       refraction constant B (radians)
   n     int           number of bodies (Note 3)
   b     iauLDBODY[n] data for each of the n bodies (Notes 3,4):
      bm    double        mass of the body (solar masses, Note 5)
      dl    double        deflection limiter (Note 6)
      pv    [2][3]        barycentric PV of the body (au, au/day)

Returned:
   ri,di   double    CIRS RA,Dec (radians)

Notes:

   1. Star data for an epoch other than J2000.0 (for example from the
   Hipparcos catalog, which has an epoch of J1991.25) will require a
   preliminary call to iauPmsafe before use.

   2. The proper motion in RA is dRA/dt rather than cos(Dec)*dRA/dt.

   3. The struct b contains n entries, one for each body to be
   considered.  If n = 0, no gravitational light deflection will be
   applied, not even for the Sun.

   4. The struct b should include an entry for the Sun as well as for
   any planet or other body to be taken into account.  The entries
   should be in the order in which the light passes the body.

   5. In the entry in the b struct for body i, the mass parameter
   b[i].bm can, as required, be adjusted in order to allow for such
   effects as quadrupole field.

   6. The deflection limiter parameter b[i].dl is phi^2/2, where phi is
   the angular separation (in radians) between star and body at
   which limiting is applied.  As phi shrinks below the chosen
   threshold, the deflection is artificially reduced, reaching zero
   for phi = 0.   Example values suitable for a terrestrial
   observer, together with masses, are as follows:

      body i     b[i].bm        b[i].dl

      Sun        1.0            6e-6
      Jupiter    0.00095435     3e-9
      Saturn     0.00028574     3e-10

   7. For efficiency, validation of the contents of the b array is
   omitted.  The supplied masses must be greater than zero, the
   position and velocity vectors must be right, and the deflection
   limiter greater than zero.

Called:
   iauPmpx      proper motion and parallax
   iauLdn       light deflection by n bodies
   iauAb        stellar aberration
   iauRxp       product of r-matrix and pv-vector
   iauC2s       p-vector to spherical
   iauAnp       normalize angle into range 0 to 2pi

This revision:   2013 October 9

SOFA release 2018-01-30

Copyright (C) 2018 IAU SOFA Board.  See notes at end.
"""
# void iauAtciqn(double rc, double dc, double pr, double pd,
#                double px, double rv, iauASTROM *astrom,
#                int n, iauLDBODY b[], double *ri, double *di)

function iauAtciqn(rc::Real, dc::Real, pr::Real, pd::Real,
                   px::Real, rv::Real, astrom::iauASTROM,
                   n::Int, b::Array{iauLDBODY, 1})

   # Allocate return value
   ref_astrom = Ref{iauASTROM}(astrom)
   ref_ri     = Ref{Float64}(0.0)
   ref_di     = Ref{Float64}(0.0)
   ref_b      = Ref{iauLDBODY}(b[1])

   # for ld in b
   #    # ld.pv = ld.pv'
   #    ld.pv = ((ld.pv[1]))
   # end

   ccall((:iauAtciqn, libsofa_c), Cvoid,
            (Cdouble, Cdouble,
            Cdouble, Cdouble,
            Cdouble, Cdouble,
            Ref{iauASTROM}, Cint, Ref{iauLDBODY},
            Ref{Cdouble}, Ref{Cdouble}),
            convert(Float64, rc), convert(Float64, dc),
            convert(Float64, pr), convert(Float64, pd),
            convert(Float64, px), convert(Float64, rv),
            ref_astrom, convert(Int32, n), ref_b, ref_ri, ref_di)

   # for ld in b
   #    ld.pv = ld.pv'
   # end

   return ref_ri[], ref_di[]
end


"""
`module  JAC.MultiPhotonDoubleIon`
... a submodel of JAC that contains all methods for computing multi-photon two-electron (double) ionization cross sections.
"""
module MultiPhotonDoubleIon

    using ..Basics, ..ManyElectron, ..Radial

    """
    `struct  MultiPhotonDoubleIon.Settings`
        ... defines a type for the settings in estimating multi-photon two-electron (double) ionization cross sections.

        + multipoles              ::Array{EmMultipole}           ... Specifies the multipoles of the radiation field that are to be included.
        + gauges                  ::Array{UseGauge}              ... Specifies the gauges to be included into the computations.
        + NoPhotons               ::Int64                        ... Number of photons in the multi-photon ionization
        + photonEnergies          ::Array{Float64,1}             ... List of photon energies.
        + printBeforeComputation  ::Bool                         ... True, if all energies and lines are printed before their evaluation.
        + selectLines             ::Bool                         ... True, if lines are selected individually for the computations.
        + selectedLines           ::Array{Tuple{Int64,Int64},1}  ... List of lines, given by tupels (inital-level, final-level).

    """
    struct Settings
        multipoles                ::Array{EmMultipole}
        gauges                    ::Array{UseGauge}
        NoPhotons                 ::Int64
        photonEnergies            ::Array{Float64,1}
        printBeforeComputation    ::Bool
        selectLines               ::Bool
        selectedLines             ::Array{Tuple{Int64,Int64},1}
    end

    """
    `MultiPhotonDoubleIon.Settings()`  ... constructor for the default values of multi-photon two-electron (double) ionization estimates.
    """
    function Settings()
        Settings(EmMultipole[], UseGauge[], 0, Float64[], false, false, Tuple{Int64,Int64}[])
    end

    # `Base.show(io::IO, settings::MultiPhotonDoubleIon.Settings)`
    #		... prepares a proper printout of the variable settings::MultiPhotonDoubleIon.Settings.
    function Base.show(io::IO, settings::MultiPhotonDoubleIon.Settings)
        println(io, "multipoles:               $(settings.multipoles)  ")
        println(io, "gauges:                   $(settings.gauges)  ")
        println(io, "NoPhotons:                $(settings.NoPhotons)  ")
        println(io, "photonEnergies:           $(settings.photonEnergies)  ")
        println(io, "printBeforeComputation:   $(settings.printBeforeComputation)  ")
        println(io, "selectLines:              $(settings.selectLines)  ")
        println(io, "selectedLines:            $(settings.selectedLines)  ")
    end

    """
    `struct  MultiPhotonDoubleIon.Channel`
        ... defines a type for a multi-photon channel to help characterize multi-photon (single-electron) ionization with
            well-defined multipolarities.

        + NoPhotons      ::EmMultipole            ... Number of photons in the multi-photon ionization
        + multipoles     ::Array{EmMultipole,1}   ... Multipoles of all N incoming photons.
        + gauge          ::EmGauge                ... Gauge for dealing with the (coupled) radiation field.
        + kappa1         ::Int64                  ... partial-wave of the first free electron
        + kappa2         ::Int64                  ... partial-wave of the second free electron
        + phase1         ::Float64                ... phase of the first partial wave
        + phase2         ::Float64                ... phase of the second partial wave
        + symmetry1      ::LevelSymmetry          ... total angular momentum and parity of scattering state after coupling of first electron
        + symmetry2      ::LevelSymmetry          ... total angular momentum and parity of scattering state after coupling of second electron
        + amplitude      ::Complex{Float64}       ... multi-photon ionization amplitude associated with the given channel.
   """
    struct  Channel
        NoPhotons        ::EmMultipole
        multipoles       ::Array{EmMultipole,1}
        gauge            ::EmGauge
        kappa1           ::Int64
        kappa2           ::Int64
        phase1           ::Float64
        phase2           ::Float64
        symmetry1        ::LevelSymmetry
        symmetry2        ::LevelSymmetry
        amplitude        ::Complex{Float64}
    end

    # MultiPhotonDoubleIon line between initial and final (bound-state) levels
    """
    `struct  MultiPhotonDoubleIon.Line`  ... defines a type for a multi-photon ionization line that may include the definition of channels.

        + initialLevel     ::Level           ... initial-(state) level
        + finalLevel       ::Level           ... final-(state) level
        + NoPhotons        ::Int64           ... Number of photons in the multi-photon process
        + photonEnergy     ::Float64         ... Energy of the incoming photons; all photons are assumed to have equal energy.
        + crossSection     ::EmProperty      ... Cross section for this multi-photon ionization.
        + hasChannels      ::Bool            ... Determines whether the individual (sub-) channels are defined in terms of
                                                 their multipolarities, etc., or not.
        + channels         ::Array{MultiPhotonDoubleIon.Channel,1}  ... List of MultiPhotonDoubleIon.Channels of this line.
    """
    struct  Line
        initialLevel       ::Level
        finalLevel         ::Level
        NoPhotons          ::Int64
        photonEnergy       ::Float64
        crossSection       ::EmProperty
        hasChannels        ::Bool
        channels           ::Array{MultiPhotonDoubleIon.Channel,1}
    end

    # `Base.show(io::IO, line::MultiPhotonDoubleIon.Line)`  ... prepares a proper printout of the variable line::MultiPhotonDoubleIon.Line.
    function Base.show(io::IO, line::MultiPhotonDoubleIon.Line)
        println(io, "initialLevel:      $(line.initialLevel)  ")
        println(io, "finalLevel:        $(line.finalLevel)  ")
        println(io, "NoPhotons:         $(line.NoPhotons)  ")
        println(io, "photonEnergy:      $(line.photonEnergy)  ")
        println(io, "crossSection:      $(line.crossSection)  ")
        println(io, "hasChannels:       $(line.hasChannels)  ")
        println(io, "channels:          $(line.channels)  ")
    end

    """
    `MultiPhotonDoubleIon.computeLines(finalMultiplet::Multiplet, initialMultiplet::Multiplet, grid::Radial.Grid,
                                       settings::MultiPhotonDoubleIon.Settings; output=true)`
        ... to compute the multiphoton transition amplitudes and all properties as requested by the given settings.
            A list of lines::Array{MultiPhotonDoubleIon.Lines} is returned.
    """
    function  computeLines(finalMultiplet::Multiplet, initialMultiplet::Multiplet, grid::Radial.Grid,
                           settings::MultiPhotonDoubleIon.Settings; output=true)
        println("")
        printstyled("MultiPhotonDoubleIon.computeLines(): The computation of multi-photon double ionization amplitudes starts now ... \n", color=:light_green)
        printstyled("----------------------------------------------------- ---------------------------------------------------------- \n", color=:light_green)
        println("")
        #
        #
        pathways = "Not yet implemented !"
        #
        if    output    return( pathways )
        else            return( nothing )
        end
    end

end # module


export
    term_pl02,
    termpt,
    timdef,
    timout,
    tipbod,
    tisbod,
    tkvrsn,
    tparse,
    tpictr,
    trace,
    tsetyr,
    twopi,
    twovec,
    tyear

"""

Find terminator points on a target body. The caller specifies
half-planes, bounded by the illumination source center-target center
vector, in which to search for terminator points.

The terminator can be either umbral or penumbral. The umbral
terminator is the boundary of the region on the target surface
where no light from the source is visible. The penumbral
terminator is the boundary of the region on the target surface
where none of the light from the source is blocked by the target
itself.

The surface of the target body may be represented either by a
triaxial ellipsoid or by topographic data.

### Arguments ###

- `method`: Computation method
- `ilusrc`: Illumination source
- `target`: Name of target body
- `et    `: Epoch in ephemeris seconds past J2000 TDB
- `fixref`: Body-fixed, body-centered target body frame
- `abcorr`: Aberration correction
- `corloc`: Aberration correction locus
- `obsrvr`: Name of observing body
- `refvec`: Reference vector for cutting half-planes
- `rolstp`: Roll angular step for cutting half-planes
- `ncuts `: Number of cutting planes
- `schstp`: Angular step size for searching
- `soltol`: Solution convergence tolerance
- `maxn  `: Maximum number of entries in output arrays

### Output ###

- `npts  `: Counts of terminator points corresponding to cuts
- `points`: Terminator points
- `epochs`: Times associated with terminator points
- `trmvcs`: Terminator vectors emanating from the observer

### References ###

- [NAIF Documentation](https://naif.jpl.nasa.gov/pub/naif/toolkit_docs/C/cspice/termpt_c.html)
"""
function termpt(method, ilusrc, target, et, fixref, abcorr, corloc, obsrvr, refvec, rolstp,
                ncuts, schstp, soltol, maxn)
    @checkdims 3 refvec
    npts = zeros(SpiceInt, maxn)
    points = Array{SpiceDouble}(undef, 3, maxn)
    epochs = Array{SpiceDouble}(undef, maxn)
    trmvcs = Array{SpiceDouble}(undef, 3, maxn)

    ccall((:termpt_c, libcspice), Cvoid,
          (Cstring, Cstring, Cstring, SpiceDouble, Cstring, Cstring, Cstring, Cstring,
           Ref{SpiceDouble}, SpiceDouble, SpiceInt, SpiceDouble, SpiceDouble, SpiceInt,
           Ref{SpiceInt}, Ref{SpiceDouble}, Ref{SpiceDouble}, Ref{SpiceDouble}),
          method, ilusrc, target, et, fixref, abcorr, corloc, obsrvr, refvec, rolstp,
          ncuts, schstp, soltol, maxn, npts, points, epochs, trmvcs)
    handleerror()
    valid_points = npts .>= 1
    npts[valid_points], cmatrix_to_array(points)[valid_points],
        epochs, cmatrix_to_array(trmvcs)[valid_points]
end

@deprecate term_pl02 termpt

"""
    timdef(action, item, value="")

Set and retrieve the defaults associated with calendar input strings.

### Arguments ###

- `action`: The kind of action to take, either `:SET` or `:GET`
- `item`: The default item of interest. The items that may be requested are:
    - `:CALENDAR` with allowed values:
        - `"GREGORIAN"`
        - `"JULIAN"`
        - `"MIXED"`
    - `:SYSTEM` with allowed values:
        - `"TDB"`
        - `"TDT"`
        - `"UTC"`
    - `:ZONE` with allowed values (`0 <= HR < 13` and `0 <= MN < 60`):
        - `"EST"`
        - `"EDT"`
        - `"CST"`
        - `"CDT"`
        - `"MST"`
        - `"MDT"`
        - `"PST"`
        - `"PDT"`
        - `"UTC+\$HR"`
        - `"UTC-\$HR"`
        - `"UTC+\$HR:\$MN"`
        - `"UTC-\$HR:\$MN"`

### Output ###

Returns the value associated with the default item.

### References ###

- [NAIF Documentation](https://naif.jpl.nasa.gov/pub/naif/toolkit_docs/C/cspice/timdef_c.html)
"""
function timdef(action, item, value="")
    lenout = 10
    val = fill(SpiceChar(0), lenout)
    val[1:length(value)] .= collect(value)
    ccall((:timdef_c, libcspice), Cvoid,
          (Cstring, Cstring, SpiceInt, Ref{SpiceChar}),
          string(action), string(item), lenout, val)
    handleerror()
    chararray_to_string(val)
end

"""
    timout(et, pictur, lenout=128)

This routine converts an input epoch represented in TDB seconds past the TDB
epoch of J2000 to a character string formatted to the specifications of a
user's format picture.

### Arguments ###

- `et`: An epoch in seconds past the ephemeris epoch J2000
- `pictur`: A format specification for the output string
- `lenout`: The length of the output string plus 1 (default: 128)

### Output ###

Returns a string representation of the input epoch.

### References ###

- [NAIF Documentation](https://naif.jpl.nasa.gov/pub/naif/toolkit_docs/C/cspice/timout_c.html)
"""
function timout(et, pictur, lenout=128)
    string = Array{SpiceChar}(undef, lenout)
    ccall((:timout_c, libcspice), Cvoid,
          (Cdouble, Cstring, SpiceInt, Ref{SpiceChar}),
          et, pictur, lenout, string)
    handleerror()
    chararray_to_string(string)
end

"""
    tipbod(ref, body, et)

Return a 3x3 matrix that transforms positions in inertial coordinates to
positions in body-equator-and-prime-meridian coordinates.

### Arguments ###

- `ref`: Name of inertial reference frame to transform from
- `body`: ID code of body
- `et`: Epoch of transformation

### Output ###

Returns transformation matrix from intertial position to prime meridian.

### References ###

- [NAIF Documentation](https://naif.jpl.nasa.gov/pub/naif/toolkit_docs/C/cspice/tipbod_c.html)
"""
function tipbod(ref, body, et)
    tipm = Array{SpiceDouble}(undef, 3, 3)
    ccall((:tipbod_c, libcspice), Cvoid,
          (Cstring, SpiceInt, SpiceDouble, Ref{SpiceDouble}),
          ref, body, et, tipm)
    handleerror()
    permutedims(tipm)
end

"""
    tisbod(ref, body, et)

Return a 6x6 matrix that transforms states in inertial coordinates to
states in body-equator-and-prime-meridian coordinates.

### Arguments ###

- `ref`: Name of inertial reference frame to transform from
- `body`: ID code of body
- `et`: Epoch of transformation

### Output ###

Returns transformation matrix from intertial state to prime meridian.

### References ###

- [NAIF Documentation](https://naif.jpl.nasa.gov/pub/naif/toolkit_docs/C/cspice/tisbod_c.html)
"""
function tisbod(ref, body, et)
    tsipm = Array{SpiceDouble}(undef, 6, 6)
    ccall((:tisbod_c, libcspice), Cvoid, (Cstring, SpiceInt, SpiceDouble, Ref{SpiceDouble}),
        ref, body, et, tsipm)
    handleerror()
    permutedims(tsipm)
end

"""
    tkvrsn(item=:TOOLKIT)

Given an item such as the Toolkit or an entry point name, return the latest
version string.

### References ###

- [NAIF Documentation](https://naif.jpl.nasa.gov/pub/naif/toolkit_docs/C/cspice/tkvrsn_c.html)
"""
function tkvrsn(item=:TOOLKIT)
    out = ccall((:tkvrsn_c, libcspice), Cstring, (Cstring,), string(item))
    return unsafe_string(out)
end

"""
    tparse(string)

Parse a time string and return seconds past the J2000 epoch on a formal calendar.

### Arguments ###

- `string`: Input time string in UTC

### Output ###

Returns UTC expressed in seconds since J2000.

### References ###

- [NAIF Documentation](https://naif.jpl.nasa.gov/pub/naif/toolkit_docs/C/cspice/tparse_c.html)
"""
function tparse(string)
    lenout = 128
    sp2000 = Ref{SpiceDouble}()
    errmsg = Array{SpiceChar}(undef, lenout)
    ccall((:tparse_c, libcspice), Cvoid,
          (Cstring, SpiceInt, Ref{SpiceDouble}, Ref{SpiceChar}),
          string, lenout, sp2000, errmsg)
    msg = chararray_to_string(errmsg)
    if !isempty(msg)
        throw(SpiceError(msg))
    end
    sp2000[]
end

"""
    tpictr(sample, lenout=80)

Given a sample time string, create a time format picture suitable for use by
the routine [`timout`](@ref).

### Arguments ###

- `sample`: A sample time string
- `lenout`: The length for the output picture string (default: 80)

### Output ###

Returns a format picture that describes sample.

### References ###

- [NAIF Documentation](https://naif.jpl.nasa.gov/pub/naif/toolkit_docs/C/cspice/tpictr_c.html)
"""
function tpictr(sample, lenout=80)
    lenerr = 128
    pictur = Array{SpiceChar}(undef, lenout)
    ok = Ref{SpiceBoolean}()
    errmsg = Array{SpiceChar}(undef, lenerr)
    ccall((:tpictr_c, libcspice), Cvoid,
          (Cstring, SpiceInt, SpiceInt, Ref{SpiceChar}, Ref{SpiceBoolean}, Ref{SpiceChar}),
          sample, lenout, lenerr, pictur, ok, errmsg)
    if !Bool(ok[])
        throw(SpiceError(chararray_to_string(errmsg)))
    end
    chararray_to_string(pictur)
end

function _trace(matrix)
    ccall((:trace_c, libcspice), SpiceDouble, (Ref{SpiceDouble},), matrix)
end

"""
    trace(matrix)

!!! warning "Deprecated"
    Use `LinearAlgebra.tr(matrix)` instead.
"""
trace

@deprecate trace LinearAlgebra.tr

"""
    tsetyr(year)

Set the lower bound on the 100 year range.

### Arguments ###

-`year`: Lower bound on the 100 year interval of expansion

### References ###

- [NAIF Documentation](https://naif.jpl.nasa.gov/pub/naif/toolkit_docs/C/cspice/tsetyr_c.html)
"""
function tsetyr(year)
    ccall((:tsetyr_c, libcspice), Cvoid, (SpiceInt,), year)
end

function _twopi()
    ccall((:twopi_c, libcspice), SpiceDouble, ())
end

"""
    twopi()

!!! warning "Deprecated"
    Use `2π` instead.
"""
twopi

@deprecate twopi() 2π

"""
    twovec(axdef, indexa, plndef, indexp)

Find the transformation to the right-handed frame having a given vector as a
specified axis and having a second given vector lying in a specified coordinate
plane.

### Arguments ###

- `axdef`: Vector defining a principal axis
- `indexa`: Principal axis number of axdef (X=1, Y=2, Z=3)
- `plndef`: Vector defining (with axdef) a principal plane
- `indexp`: Second axis number (with indexa) of principal plane

### Output ###

Returns output rotation matrix.

### References ###

- [NAIF Documentation](https://naif.jpl.nasa.gov/pub/naif/toolkit_docs/C/cspice/twovec_c.html)
"""
function twovec(axdef, indexa, plndef, indexp)
    @checkdims 3 axdef plndef
    mout = Array{SpiceDouble}(undef, 3, 3)
    ccall((:twovec_c, libcspice), Cvoid,
          (Ref{SpiceDouble}, SpiceInt, Ref{SpiceDouble}, SpiceInt, Ref{SpiceDouble}),
          axdef, indexa, plndef, indexp, mout)
    handleerror()
    permutedims(mout)
end

"""
    tyear()

Returns the number of seconds per tropical year.

### References ###

- [NAIF Documentation](https://naif.jpl.nasa.gov/pub/naif/toolkit_docs/C/cspice/tyear_c.html)
"""
function tyear()
    ccall((:tyear_c, libcspice), SpiceDouble, ())
end


using OrdinaryDiffEq: OrdinaryDiffEq

"""
    Classical <: DynamicsMethods.Method

Type for performing classical molecular dynamics.

```jldoctest
sim = Simulation{Classical}(Atoms(:H), Harmonic())

# output

Simulation{Classical}:
  Atoms{Float64}([:H], [1], [1837.4715941070515])
  Harmonic{Float64, Float64, Float64}
  m: Float64 1.0
  ω: Float64 1.0
  r₀: Float64 0.0
  dofs: Int64 1
```
"""
struct Classical <: DynamicsMethods.Method end

function NQCDynamics.Simulation{Classical}(atoms::Atoms, model::Model; kwargs...)
    NQCDynamics.Simulation(atoms, model, Classical(); kwargs...)
end
function NQCDynamics.RingPolymerSimulation{Classical}(atoms::Atoms, model::Model, n_beads::Integer; kwargs...)
    NQCDynamics.RingPolymerSimulation(atoms, model, Classical(), n_beads::Integer; kwargs...)
end

function NQCDynamics.Simulation(atoms::Atoms, model::Model; kwargs...)
    NQCDynamics.Simulation{Classical}(atoms, model; kwargs...)
end
function NQCDynamics.RingPolymerSimulation(atoms::Atoms, model::Model, n_beads::Integer; kwargs...)
    NQCDynamics.RingPolymerSimulation{Classical}(atoms, model, n_beads; kwargs...)
end

function DynamicsMethods.motion!(du, u, sim::AbstractSimulation{<:Classical}, t)
    dr = DynamicsUtils.get_positions(du)
    dv = DynamicsUtils.get_velocities(du)
    r = DynamicsUtils.get_positions(u)
    v = DynamicsUtils.get_velocities(u)
    DynamicsUtils.velocity!(dr, v, r, sim, t)
    acceleration!(dv, v, r, sim, t)
end

function DynamicsMethods.motion!(du, u, sim::RingPolymerSimulation{<:Classical}, t)
    dr = DynamicsUtils.get_positions(du)
    dv = DynamicsUtils.get_velocities(du)
    r = DynamicsUtils.get_positions(u)
    v = DynamicsUtils.get_velocities(u)
    DynamicsUtils.velocity!(dr, v, r, sim, t)
    ring_polymer_acceleration!(dv, v, r, sim, t)
end

"""
`f1` in `DifferentialEquations.jl` docs.
"""
function acceleration!(dv, v, r, sim::AbstractSimulation, t)
    Calculators.evaluate_derivative!(sim.calculator, r)
    dv .= -sim.calculator.derivative
    DynamicsUtils.divide_by_mass!(dv, sim.atoms.masses)
end

function ring_polymer_acceleration!(dv, v, r, sim::RingPolymerSimulation, t)
    Calculators.evaluate_derivative!(sim.calculator, r)
    dv .= -sim.calculator.derivative
    DynamicsUtils.divide_by_mass!(dv, sim.atoms.masses)
    DynamicsUtils.apply_interbead_coupling!(dv, r, sim)
end

function DynamicsMethods.create_problem(u0, tspan::Tuple, sim::Simulation{<:Classical})
    OrdinaryDiffEq.DynamicalODEProblem(acceleration!, DynamicsUtils.velocity!,
        DynamicsUtils.get_velocities(u0), DynamicsUtils.get_positions(u0), tspan, sim)
end

function DynamicsMethods.create_problem(u0, tspan::Tuple, sim::RingPolymerSimulation{<:Classical})
    OrdinaryDiffEq.DynamicalODEProblem(ring_polymer_acceleration!, DynamicsUtils.velocity!,
        DynamicsUtils.get_velocities(u0), DynamicsUtils.get_positions(u0), tspan, sim)
end

DynamicsMethods.select_algorithm(::AbstractSimulation{<:Classical}) = OrdinaryDiffEq.VelocityVerlet()