module ClpCInterface

using SparseArrays

export
    # Types
    CoinBigIndex,
    CoinBigDouble,
    ClpModel,
    ClpSolve,

    # Methods
    load_problem,
    load_quadratic_objective,
    read_mps,
    copy_in_integer_information,
    delete_integer_information,
    resize,
    delete_rows,
    add_rows,
    add_row,
    delete_columns,
    add_column,
    add_columns,
    chg_row_lower,
    chg_row_upper,
    chg_column_lower,
    chg_column_upper,
    chg_obj_coefficients,
    drop_names,
    copy_names,
    get_num_rows,
    get_num_cols,
    number_rows,
    number_cols,
    primal_tolerance,
    set_primal_tolerance,
    dual_tolerance,
    set_dual_tolerance,
    dual_objective_limit,
    set_dual_objective_limit,
    objective_offset,
    set_objective_offset,
    problem_name,
    set_problem_name,
    number_iterations,
    get_iteration_count,
    set_number_iterations,
    maximum_iterations,
    set_maximum_iterations,
    maximum_seconds,
    set_maximum_seconds,
    hit_maximum_iterations,
    status,
    set_problem_status,
    secondary_status,
    set_secondary_status,
    optimization_direction,
    set_optimization_direction,
    get_row_activity,
    get_col_solution,
    primal_column_solution,
    set_col_solution,
    primal_row_solution,
    get_row_price,
    dual_row_solution,
    get_reduced_cost,
    dual_column_solution,
    get_row_lower,
    row_lower,
    get_row_upper,
    row_upper,
    get_obj_coefficients,
    objective,
    get_col_lower,
    column_lower,
    get_col_upper,
    column_upper,
    get_num_elements,
    get_vector_starts,
    get_indices,
    get_constraint_matrix,
    get_vector_lengths,
    get_elements,
    get_obj_value,
    objective_value,
    integer_information,
    infeasibility_ray,
    unbounded_ray,
    statusExists,
    status_array,
    copyin_status,
    get_column_status,
    get_row_status,
    set_column_status,
    set_row_status,
    register_call_back,
    clear_call_back,
    log_level,
    set_log_level,
    length_names,
    row_name,
    column_name,
    initial_solve,
    initial_solve_with_options,
    initial_dual_solve,
    initial_primal_solve,
    initial_barrier_solve,
    initial_barrier_no_cross_solve,
    dual,
    primal,
    idiot,
    scaling,
    scaling_flag,
    crash,
    primal_feasible,
    dual_feasible,
    dual_bound,
    set_dual_bound,
    infeasibility_cost,
    set_infeasibility_cost,
    perturbation,
    set_perturbation,
    algorithm,
    set_algorithm,
    sum_dual_infeasibilities,
    number_dual_infeasibilities,
    sum_primal_infeasibilities,
    number_primal_infeasibilities,
    save_model,
    restore_model,
    check_solution,
    is_abandoned,
    is_proven_optimal,
    is_proven_primal_infeasible,
    is_proven_dual_infeasible,
    is_primal_objective_limit_reached,
    is_dual_objective_limit_reached,
    is_iteration_limit_reached,
    get_obj_sense,
    set_obj_sense,
    print_model,
    get_small_element_value,
    set_small_element_value,

    # ClpSolve methods
    set_special_option,
    get_special_option,
    set_presolve_type,
    get_presolve_type,
    set_solve_type,
    get_solve_type,
    get_presolve_passes,
    get_extra_info,
    set_infeasible_return,
    infeasible_return,
    do_dual,
    set_do_dual,
    do_singleton,
    set_do_singleton,
    do_doubleton,
    set_do_doubleton,
    do_tripleton,
    set_do_tripleton,
    do_tighten,
    set_do_tighten,
    do_forcing,
    set_do_forcing,
    do_implied_free,
    set_do_implied_free,
    do_dupcol,
    set_do_dupcol,
    do_duprow,
    set_do_duprow,
    do_singleton_column,
    set_do_singleton_column,
    presolve_actions,
    set_presolve_actions,
    substitution,
    set_substitution

import Base.pointer

if haskey(ENV,"JULIA_CLP_LIBRARY_PATH") || VERSION < v"1.3"
    if isfile(joinpath(dirname(@__FILE__),"..","deps","deps.jl"))
        include("../deps/deps.jl")
    else
        error("Clp not properly installed. Please run import Pkg; Pkg.build(\"Clp\")")
    end
else
    import Clp_jll: libClp
end
## Shared library interface setup
#{{{

macro clp_ccall(func, args...)
    args = [esc(ex) for ex in args]
    f = "Clp_$(func)"
    quote
        ccall(($f,libClp), $(args...))
    end
end

macro clpsolve_ccall(func, args...)
    args = [esc(ex) for ex in args]
    f = "ClpSolve_$(func)"
    quote
        ccall(($f,libClp), $(args...))
    end
end

# Note: we assume COIN_BIG_INDEX and COIN_BIG_DOUBLE
# were not defined when compiling Clp (which is the
# default)
const CoinBigIndex  = Int32
const CoinBigDouble = Float64

#}}}

## Main types definitions
#{{{
mutable struct ClpModel
    p::Ptr{Cvoid}
    function ClpModel()
        p = @clp_ccall newModel Ptr{Cvoid} ()
        prob = new(p)
        finalizer(delete_model, prob)
        return prob
    end
end

function delete_model(model::ClpModel)
    if model.p == C_NULL
        return
    end
    @clp_ccall deleteModel Cvoid (Ptr{Cvoid},) model.p
    model.p = C_NULL
    return
end

mutable struct ClpSolve
    p::Ptr{Cvoid}
    function ClpSolve()
        p = @clpsolve_ccall new Ptr{Cvoid} ()
        prob = new(p)
        finalizer(delete_solve, prob)
        return prob
    end
end

function delete_solve(solve::ClpSolve)
    if solve.p == C_NULL
        return
    end
    @clpsolve_ccall delete Cvoid (Ptr{Cvoid},) solve.p
    solve.p = C_NULL
    return
end

pointer(model::ClpModel) = model.p
#}}}

## Check functions for internal use
#{{{
# Functions which perform all sorts of
# sanity checks on input parameters and
# throw exceptions in case of errors.
# Ideally, it should never be possible
# to pass an invalid parameter to the
# underlying Clp API.

function _jl__check_model(model::ClpModel)
    if model.p == C_NULL
        error("Invalid ClpModel")
    end
    return true
end

function _jl__check_solve(solve::ClpSolve)
    if solve.p == C_NULL
        error("Invalid ClpSolve")
    end
    return true
end

function _jl__check_row_is_valid(model::ClpModel, row::Integer)
    num_rows = @clp_ccall getNumRows Int32 (Ptr{Cvoid},) model.p
    if !(1 <= row <= num_rows)
        error("Invalid row $row (must be 1 <= row <= $num_rows)")
    end
    return true
end

function _jl__check_col_is_valid(model::ClpModel, col::Integer)
    num_cols = @clp_ccall getNumCols Int32 (Ptr{Cvoid},) model.p
    if !(1 <= col <= num_cols)
        error("Invalid col $col (must be 1 <= col <= $num_cols)")
    end
    return true
end

function _jl__check_file_is_readable(filename::String)
    try
        f = open(filename, "r")
        close(f)
    catch err
        error("file $filename not readable")
    end
    return true
end
#}}}

## CLP functions
#{{{

# inspired by GLPK interface
const VecOrNothing = Union{Vector,Cvoid}
function vec_or_null(::Type{T}, a::VecOrNothing, len::Integer) where T
    if isequal(a, nothing) || isa(a, Array{Cvoid}) # on 0.3, [] is Array{Cvoid}
        return C_NULL
    else # todo: helpful message if convert fails
        if length(a) != len
            error("Expected vector to have length $len")
        end
        return convert(Vector{T},a)
    end
end

# Load model - loads some stuff and initializes others
# Loads a problem (the constraints on the
# rows are given by lower and upper bounds). If a pointer is NULL then the
# following values are the default:
#
# col_ub: all columns have upper bound infinity
# col_lb: all columns have lower bound 0
# row_ub: all rows have upper bound infinity
# row_lb: all rows have lower bound -infinity
# obj: all variables have 0 objective coefficient

# Just like the other load_problem() method except that the matrix is
# given in a standard column major ordered format (without gaps).
function load_problem(model::ClpModel,  num_cols::Integer, num_rows::Integer,
        start::Vector{CoinBigIndex}, index::Vector{Int32},
        value::Vector{Float64},
        col_lb::VecOrNothing, col_ub::VecOrNothing,
        obj::VecOrNothing,
        row_lb::VecOrNothing, row_ub::VecOrNothing)
    _jl__check_model(model)
    @clp_ccall loadProblem Cvoid (Ptr{Cvoid},Int32,Int32,Ptr{CoinBigIndex},Ptr{Int32},
    Ptr{Float64},Ptr{Float64},Ptr{Float64},Ptr{Float64},Ptr{Float64},Ptr{Float64}) model.p num_cols num_rows start index value vec_or_null(Float64,col_lb,num_cols) vec_or_null(Float64,col_ub,num_cols) vec_or_null(Float64,obj,num_cols) vec_or_null(Float64,row_lb,num_rows) vec_or_null(Float64,row_ub,num_rows)
end

function load_problem(model::ClpModel,  constraint_matrix::AbstractMatrix,
    col_lb::VecOrNothing, col_ub::VecOrNothing,
    obj::VecOrNothing, row_lb::VecOrNothing,
    row_ub::VecOrNothing)
    mat = convert(SparseMatrixCSC{Float64,Int32},constraint_matrix)
    # We need to convert to zero-based, but
    # TODO: don't make extra copies of arrays
    # TODO: check dimensions match
    load_problem(model, mat.n, mat.m, mat.colptr .- convert(Int32,1),
                 mat.rowval .- convert(Int32, 1), mat.nzval,
                 col_lb, col_ub,obj, row_lb, row_ub)
end

# Read quadratic part of the objective (the matrix part).
function load_quadratic_objective(model::ClpModel,
        num_cols::Integer, start::Vector{CoinBigIndex},
        col::Vector{Int32}, element::Vector{Float64})
    _jl__check_model(model)
    @clp_ccall loadQuadraticObjective Cvoid (Ptr{Cvoid},Int32,Ptr{CoinBigIndex},Ptr{Int32},Ptr{Float64}) model.p num_cols start column element
end

function load_quadratic_objective(model::ClpModel,
    hessian_matrix::SparseMatrixCSC{Float64,Int32})
    load_quadratic_objective(model, hessian_matrix.n, hessian_matrix.colptr-convert(Int32,1), hessian_matrix.rowval-convert(Int32,1),hessian_matrix.nzval)
end

# Read an mps file from the given filename.
function read_mps(model::ClpModel, mpsfile::AbstractString, keep_names::Bool, ignore_errors::Bool)
    _jl__check_model(model)
    _jl__check_file_is_readable(mpsfile)

    status = @clp_ccall readMps Int32 (Ptr{Cvoid}, Cstring, Int32, Int32) model.p mpsfile keep_names ignore_errors
    if status != 0
        error("read_mps: error reading file $mpsfile")
    end
    return true
end
read_mps(model::ClpModel, mpsfile::AbstractString) = read_mps(model, mpsfile, true, false)

# Copy in integer information.
function copy_in_integer_information(model::ClpModel, information::Vector{UInt8})
    _jl__check_model(model)
    if length(information) != num_cols(model)
        error("Length of integer information must match number of columns")
    end
    @clp_ccall copyInIntegerInformation Cvoid (Ptr{Cvoid},Ptr{UInt8}) model.p information
end

# Drop integer information.
function delete_integer_information(model::ClpModel)
    _jl__check_model(model)
    @clp_ccall deleteIntegerInformation Cvoid (Ptr{Cvoid},) model.p
end

# Resize rim part of model.
function resize(model::ClpModel, new_num_rows::Integer, new_num_cols::Integer)
    _jl__check_model(model)
    @clp_ccall resize Cvoid (Ptr{Cvoid},Int32,Int32) model.p new_num_rows new_num_cols
end

# Delete rows.
function delete_rows(model::ClpModel, which::Vector{Int32})
    _jl__check_model(model)
    @clp_ccall deleteRows Cvoid (Ptr{Cvoid},Int32,Ptr{Int32}) model.p length(which) which
end

# Add rows.
function add_rows(model::ClpModel, number::Integer, row_lower::Vector{Float64},
        row_upper::Vector{Float64},
        row_starts::Vector{Int32}, columns::Vector{Int32},
        elements::Vector{Float64})
    _jl__check_model(model)
    @clp_ccall addRows Cvoid (Ptr{Cvoid}, Int32, Ptr{Float64}, Ptr{Float64}, Ptr{Int32}, Ptr{Int32}, Ptr{Float64}) model.p number row_lower row_upper row_starts columns elements
end

#This function exists in cpp but not c interface
function add_row(model::ClpModel, nnz::Cint, columns::Vector{Int32},
                 elements::Vector{Float64}, row_lower::Float64, row_upper::Float64)
    add_rows(model, 1, [row_lower], [row_upper], [Cint(0), nnz], columns,
             elements)
end

# Delete columns.
function delete_columns(model::ClpModel, which::Vector{Int32})
    _jl__check_model(model)
    @clp_ccall deleteColumns Cvoid (Ptr{Cvoid},Int32,Ptr{Int32}) model.p length(which) which
end

#This function exists in cpp but not c interface
function add_column(model::ClpModel, nnz::Int32,  rows::Vector{Int32},
                elements::Vector{Float64}, column_lower::Float64,
                column_upper::Float64, objective::Float64)
    add_columns(model, 1, [column_lower], [column_upper], [objective],
                [Int32(0), nnz], rows, elements)
end

# Add columns.
function add_columns(model::ClpModel, number::Integer, column_lower::Vector{Float64},
        column_upper::Vector{Float64},
        objective::Vector{Float64},
        column_starts::Vector{Int32}, rows::Vector{Int32},
        elements::Vector{Float64})
    _jl__check_model(model)

    @clp_ccall addColumns Cvoid (Ptr{Cvoid}, Int32, Ptr{Float64}, Ptr{Float64}, Ptr{Float64}, Ptr{Int32}, Ptr{Int32}, Ptr{Float64}) model.p number column_lower column_upper objective column_starts rows elements

end

function add_columns(model::ClpModel, column_lower::Vector{Float64},
        column_upper::Vector{Float64},
        objective::Vector{Float64},
        new_columns::SparseMatrixCSC{Float64,Int32})
    add_columns(model, new_columns.n, column_lower, column_upper, objective, new_columns.colptr-convert(Int32,1), new_columns.rowval-convert(Int32,1), new_columns.nzval)
end

# Change row lower bounds.
function chg_row_lower(model::ClpModel, row_lower::Vector{Float64})
    _jl__check_model(model)
    if length(row_lower) != get_num_rows(model)
        error("Array length must match number of rows in the model")
    end

    @clp_ccall chgRowLower Cvoid (Ptr{Cvoid}, Ptr{Float64}) model.p row_lower
end

# Change row upper bounds.
function chg_row_upper(model::ClpModel, row_upper::Vector{Float64})
    _jl__check_model(model)
    if length(row_upper) != get_num_rows(model)
        error("Array length must match number of rows in the model")
    end

    @clp_ccall chgRowUpper Cvoid (Ptr{Cvoid}, Ptr{Float64}) model.p row_upper
end

# Change column lower bounds.
function chg_column_lower(model::ClpModel, column_lower::Vector{Float64})
    _jl__check_model(model)
    if length(column_lower) != get_num_cols(model)
        error("Array length must match number of columns in the model")
    end

    @clp_ccall chgColumnLower Cvoid (Ptr{Cvoid}, Ptr{Float64}) model.p column_lower
end

# Change column upper bounds.
function chg_column_upper(model::ClpModel, column_upper::Vector{Float64})
    _jl__check_model(model)
    if length(column_upper) != get_num_cols(model)
        error("Array length must match number of columns in the model")
    end

    @clp_ccall chgColumnUpper Cvoid (Ptr{Cvoid}, Ptr{Float64}) model.p column_upper
end

# Change objective coefficients.
function chg_obj_coefficients(model::ClpModel, obj_in::Vector{Float64})
    _jl__check_model(model)
    if length(obj_in) != get_num_cols(model)
        error("Array length must match number of columns in the model")
    end

    @clp_ccall chgObjCoefficients Cvoid (Ptr{Cvoid},Ptr{Float64}) model.p obj_in
end

# Drops names - makes lengthnames 0 and names empty.
function drop_names(model::ClpModel)
    _jl__check_model(model)

    @clp_ccall dropNames Cvoid (Ptr{Cvoid},) model.p
end

# Copy in names.
# TODO: Need a nice way to accept a vector of Strings
function copy_names(model::ClpModel, row_names::Vector{Vector{UInt8}},
        columnNames::Vector{Vector{UInt8}})
    # TODO
    error("TODO")
    return
end

# Number of rows.
function get_num_rows(model::ClpModel)
    _jl__check_model(model)
    @clp_ccall getNumRows Int32 (Ptr{Cvoid},) model.p
end
const number_rows = get_num_rows

# Number of columns.
function get_num_cols(model::ClpModel)
    _jl__check_model(model)
    @clp_ccall getNumCols Int32 (Ptr{Cvoid},) model.p
end
const number_cols = get_num_cols

# Get primal tolerance.
function primal_tolerance(model::ClpModel)
    _jl__check_model(model)
    @clp_ccall primalTolerance Float64 (Ptr{Cvoid},) model.p
end

# Set primal tolerance to use.
function set_primal_tolerance(model::ClpModel, value::Float64)
    _jl__check_model(model)
    @clp_ccall setPrimalTolerance Cvoid (Ptr{Cvoid}, Float64) model.p value
    return
end

# Get dual tolerance.
function dual_tolerance(model::ClpModel)
    _jl__check_model(model)
    @clp_ccall dualTolerance Float64 (Ptr{Cvoid},) model.p
end

# Set dual tolerance to use.
function set_dual_tolerance(model::ClpModel, value::Float64)
    _jl__check_model(model)
    @clp_ccall setDualTolerance Cvoid (Ptr{Cvoid}, Float64) model.p value
    return
end

# Get dual objective limit.
function dual_objective_limit(model::ClpModel)
    _jl__check_model(model)
    @clp_ccall dualObjectiveLimit Float64 (Ptr{Cvoid},) model.p
end

# Set dual objective limit.
function set_dual_objective_limit(model::ClpModel, value::Float64)
    @clp_ccall setDualObjectiveLimit Cvoid (Ptr{Cvoid}, Float64) model.p value
    return
end

# Get objective offset.
function objective_offset(model::ClpModel)
    _jl__check_model(model)
    @clp_ccall dualObjectiveLimit Float64 (Ptr{Cvoid},) model.p
end

# Set objective offset.
function set_objective_offset(model::ClpModel, value::Float64)
    @clp_ccall setObjectiveOffset Cvoid (Ptr{Cvoid}, Float64) model.p value
    return
end

#=
These methods are untested. If you need them, open a PR to get them working
and add new unit tests.
# Fill in array with problem name.
function problem_name(model::ClpModel)
    _jl__check_model(model)
    problem_name = Array{UInt8}(undef, 1000)
    @clp_ccall problemName Cvoid (Ptr{Cvoid}, Int32, Ptr{UInt8}) model.p 1000 problem_name
    return bytestring(problem_name)
end

# Set problem name.  Must have \0 at end.
function set_problem_name(model::ClpModel, name::String)
    _jl__check_model(model)
    @assert isascii(name)

    @clp_ccall setProblemName Cvoid (Ptr{Cvoid}, Int32, Ptr{UInt8}) model.p (length(name)+1) bytestring(name)
end
=#

# Get number of iterations
function number_iterations(model::ClpModel)
    _jl__check_model(model)
    @clp_ccall numberIterations Int32 (Ptr{Cvoid},) model.p
end
const get_iteration_count = number_iterations

# Set number of iterations
function set_number_iterations(model::ClpModel, iters::Integer)
    _jl__check_model(model)
    @clp_ccall setNumberIterations Cvoid (Ptr{Cvoid}, Int32) model.p iters
    return
end

# Get maximum number of iterations
function maximum_iterations(model::ClpModel)
    _jl__check_model(model)
    # The C function's name does not follow the `Clp_` convention
    # See https://github.com/JuliaOpt/Clp.jl/issues/67
    ccall((:maximumIterations, libClp), Int32, (Ptr{Cvoid},), model.p)
end

# Set maximum number of iterations
function set_maximum_iterations(model::ClpModel, max_iters::Integer)
    _jl__check_model(model)
    @clp_ccall setMaximumIterations Cvoid (Ptr{Cvoid}, Int32) model.p max_iters
    return
end

# Get maximum time in seconds (from when set is called)
function maximum_seconds(model::ClpModel)
    _jl__check_model(model)
    @clp_ccall maximumSeconds Float64 (Ptr{Cvoid},) model.p
end

# Set maximum time in seconds (from when set is called)
function set_maximum_seconds(model::ClpModel, max_secs::Real)
    _jl__check_model(model)
    @clp_ccall setMaximumSeconds Cvoid (Ptr{Cvoid}, Float64) model.p max_secs
    return
end

# Query whether maximum iterations or time were hit
function hit_maximum_iterations(model::ClpModel)
    _jl__check_model(model)
    ret = @clp_ccall hitMaximumIterations Int32 (Ptr{Cvoid},) model.p
    return ret != 0
end

# Get the status of the problem:
#   0 - optimal
#   1 - primal infeasible
#   2 - dual infeasible
#   3 - stopped on iterations etc
#   4 - stopped due to errors
function status(model::ClpModel)
    _jl__check_model(model)
    @clp_ccall status Int32 (Ptr{Cvoid},) model.p
end

# Set the status of the problem.
function set_problem_status(model::ClpModel, status::Integer)
    _jl__check_model(model)
    @clp_ccall setProblemStatus Cvoid (Ptr{Cvoid}, Int32) model.p status
    return
end

# Get the secondary status of problem - may get extended:
#   0 - none
#   1 - primal infeasible because dual limit reached
#   2 - scaled problem optimal - unscaled has primal infeasibilities
#   3 - scaled problem optimal - unscaled has dual infeasibilities
#   4 - scaled problem optimal - unscaled has both dual and primal infeasibilities
function secondary_status(model::ClpModel)
    _jl__check_model(model)
    @clp_ccall secondaryStatus Int32 (Ptr{Cvoid},) model.p
end

# Set the secondary status of the problem.
function set_secondary_status(model::ClpModel, status::Integer)
    _jl__check_model(model)
    @clp_ccall setSecondaryStatus Cvoid (Ptr{Cvoid}, Int32) model.p status
    return
end

# Get the direction of optimization:
#   1 - minimize
#   -1 - maximize
#   0 - ignore
# XXX: for some reason this returns a floating point (???)
function optimization_direction(model::ClpModel)
    _jl__check_model(model)
    @clp_ccall optimizationDirection Float64 (Ptr{Cvoid},) model.p
end

# Set the direction of optimization.
# XXX: for some reason this takes a floating point argument (???)
function set_optimization_direction(model::ClpModel, value::Real)
    _jl__check_model(model)
    @clp_ccall setOptimizationDirection Cvoid (Ptr{Cvoid}, Float64) model.p value
    return
end

# TODO: do we actually want to make a copy of the result?
# More efficient to not and let the sufficiently warned user do so if desired.
macro def_get_row_property(fname,clpname)
    return quote
        function $(esc(fname))(model::ClpModel)
            _jl__check_model(model)
            row_property_p = @clp_ccall $clpname Ptr{Float64} (Ptr{Cvoid},) model.p
            num_rows = convert(Int,get_num_rows(model))
            return copy(unsafe_wrap(Array,row_property_p,(num_rows,)))
        end
    end
end

macro def_get_col_property(fname,clpname)
    return quote
        function $(esc(fname))(model::ClpModel)
            _jl__check_model(model)
            col_property_p = @clp_ccall $clpname Ptr{Float64} (Ptr{Cvoid},) model.p
            num_cols = convert(Int,get_num_cols(model))
            return copy(unsafe_wrap(Array,col_property_p,(num_cols,)))
        end
    end
end

# Get primal row solution.
@def_get_row_property get_row_activity getRowActivity

const primal_row_solution = get_row_activity

# Get primal column solution.
@def_get_col_property get_col_solution getColSolution
const primal_column_solution = get_col_solution

function set_col_solution(model::ClpModel, input::Vector{Float64})
    _jl__check_model(model)

    @clp_ccall setColSolution Cvoid (Ptr{Cvoid},Ptr{Float64}) model.p input
end

# Get dual row solution.
@def_get_row_property get_row_price getRowPrice
const dual_row_solution = get_row_price

# Get dual column solution (i.e. the reduced costs).
@def_get_col_property get_reduced_cost getReducedCost
const dual_column_solution = get_reduced_cost

# Get row lower bounds.
@def_get_row_property get_row_lower getRowLower
const row_lower = get_row_lower

# Get row upper bounds.
@def_get_row_property get_row_upper getRowUpper
const row_upper = get_row_upper

@def_get_col_property get_obj_coefficients getObjCoefficients
const objective = get_obj_coefficients

# Get column lower bounds.
@def_get_col_property get_col_lower getColLower
const column_lower = get_col_lower

# Get column upper bounds.
@def_get_col_property get_col_upper getColUpper
const column_upper = get_col_upper

# Get the number of elements in matrix.
function get_num_elements(model::ClpModel)
    _jl__check_model(model)
    @clp_ccall getNumElements Int32 (Ptr{Cvoid},) model.p
end

# Get column starts in matrix.
function get_vector_starts(model::ClpModel)
    _jl__check_model(model)
    vec_starts_p = @clp_ccall getVectorStarts Ptr{CoinBigIndex} (Ptr{Cvoid},) model.p
    num_cols = Int(get_num_cols(model))
    return copy(unsafe_wrap(Array,vec_starts_p, (num_cols+1,)))
end

# Get row indices in matrix.
function get_indices(model::ClpModel)
    _jl__check_model(model)
    # getIndices returns an "int*", how do we know it's Int32??
    row_indices_p = @clp_ccall getIndices Ptr{Int32} (Ptr{Cvoid},) model.p
    num_elts = Int(get_num_elements(model))
    return copy(unsafe_wrap(Array,row_indices_p,(num_elts,)))
end

function get_constraint_matrix(model::ClpModel)
    _jl__check_model(model)
    @assert CoinBigIndex == Int32
    # SparseMatrixCSC requires same integer type for colptr and rowval
    num_cols = convert(Int, get_num_cols(model))
    num_rows = convert(Int, get_num_rows(model))
    colptr = get_vector_starts(model) .+ convert(Int32,1)
    rowval = get_indices(model) .+ convert(CoinBigIndex,1)
    nzval = get_elements(model)

    return SparseMatrixCSC{Float64,CoinBigIndex}(num_rows,num_cols,colptr,rowval,nzval)

end

# Get column vector lengths in matrix.
@def_get_col_property get_vector_lengths getVectorLengths

# Get element values in matrix.
function get_elements(model::ClpModel)
    _jl__check_model(model)
    elements_p = @clp_ccall getElements Ptr{Float64} (Ptr{Cvoid},) model.p
    num_elts = Int(get_num_elements(model))
    return copy(unsafe_wrap(Array,elements_p,(num_elts,)))
end

# Get objective value.
function get_obj_value(model::ClpModel)
    _jl__check_model(model)
    @clp_ccall getObjValue Float64 (Ptr{Cvoid},) model.p
end
const objective_value = get_obj_value

# Get integer information.
function integer_information(model::ClpModel)
    # TODO
    error("TODO")
    return # vector of chars (?)
end

# Get an infeasibility ray (empty vector returned if none/wrong).
function infeasibility_ray(model::ClpModel)
    _jl__check_model(model)
    infeas_ray_p = @clp_ccall(
        infeasibilityRay, Ptr{Float64}, (Ptr{Cvoid},), model.p
    )
    if infeas_ray_p == C_NULL
        return Float64[]
    end
    num_rows = convert(Int, get_num_rows(model))
    return unsafe_wrap(Array, infeas_ray_p, (num_rows,); own = true)
end

# Get an unbounded ray (empty vector returned if none/wrong).
function unbounded_ray(model::ClpModel)
    _jl__check_model(model)
    unbd_ray_p = @clp_ccall(unboundedRay, Ptr{Float64}, (Ptr{Cvoid},), model.p)
    if unbd_ray_p == C_NULL
        return Float64[]
    end
    num_cols = convert(Int, get_num_cols(model))
    return unsafe_wrap(Array, unbd_ray_p, (num_cols,); own = true)
end

# Query whether the status array exists (partly for OsiClp).
function statusExists(model::ClpModel)
    _jl__check_model(model)
    ret = @clp_ccall statusExists Int32 (Ptr{Cvoid},) model.p
    return ret != 0
end

# Return the address of the status array (char[num_rows+num_cols]).
# Status values are:
#   0 - free
#   1 - basic
#   2 - at upper
#   3 - at lower
#   4 - superbasic
#   (5 - fixed)
#   [???]
function status_array(model::ClpModel)
    # TODO
    error("TODO")
    return # pointer to unsigned chars (?)
end

# Copy in the status vector
# XXX copyin rather than copy_in, WTF?
# XXX input vector of unsigned chars (?)
function copyin_status(model::ClpModel, status_array::Vector{UInt8})
    # TODO
    error("TODO")
    return
end

# Get variable basis info.
# Note that we adjust from one-based indices
function get_column_status(model::ClpModel, sequence::Integer)
    _jl__check_model(model)
    _jl__check_col_is_valid(model, sequence)
    return @clp_ccall getColumnStatus Int32 (Ptr{Cvoid},Int32) model.p (sequence-1)
end

# Get row basis info.
function get_row_status(model::ClpModel, sequence::Integer)
    _jl__check_model(model)
    _jl__check_row_is_valid(model, sequence)
    return @clp_ccall getRowStatus Int32 (Ptr{Cvoid},Int32) model.p (sequence-1)
end

# Set variable basis info (and value if at bound).
function set_column_status(model::ClpModel, sequence::Integer, value::Integer)
    _jl__check_model(model)
    _jl__check_col_is_valid(model, sequence)
    @clp_ccall setColumnStatus Cvoid (Ptr{Cvoid},Int32,Int32) model.p (sequence-1) value
end

# Set row basis info (and value if at bound).
function set_row_status(model::ClpModel, sequence::Integer, value::Integer)
    _jl__check_model(model)
    _jl__check_row_is_valid(model, sequence)
    @clp_ccall setRowStatus Cvoid (Ptr{Cvoid},Int32,Int32) model.p (sequence-1) value
end

# No reason to expose these
# Set user pointer (used for generic purposes)
#function get_user_pointer(model::ClpModel)
# Get user pointer (used for generic purposes)
#function set_user_pointer(model::ClpModel, pointer::Ptr{Cvoid})

# Pass in c-pointer to callback function. (from cfunction())
# See Coin_C_defines.h for signature
# TODO: test this
function register_call_back(model::ClpModel, callback::Ptr{Cvoid})
    _jl__check_model(model)
    @clp_ccall registerCallBack Cvoid (Ptr{Cvoid},Ptr{Cvoid}) model.p callback
end

# Unset callback function.
function clear_call_back(model::ClpModel)
    _jl__check_model(model)
    @clp_ccall clearCallBack Cvoid (Ptr{Cvoid},) model.p
    return
end

# Get amount of print out:
#   0 - none
#   1 - just final
#   2 - just factorizations
#   3 - as 2 plus a bit more
#   4 - verbose
#   above that: 8,16,32 etc just for selective debug.
function log_level(model::ClpModel)
    _jl__check_model(model)
    @clp_ccall logLevel Int32 (Ptr{Cvoid},) model.p
end

# Set amount of print out.
function set_log_level(model::ClpModel, value::Integer)
    _jl__check_model(model)
    @clp_ccall setLogLevel Cvoid (Ptr{Cvoid}, Int32) model.p value
    return
end

# Get length of names (0 means no names).
function length_names(model::ClpModel)
    _jl__check_model(model)
    return @clp_ccall lengthNames Int32 (Ptr{Cvoid},) model.p
end

# Return an array with a row name.
# XXX deviation from Clp API syntax, which fills a user-provided
# array of chars:
#   void Clp_rowName(Clp_Simplex * model, int iRow, char * name);
function row_name(model::ClpModel, row::Integer)
    _jl__check_model(model)
    _jl__check_row_is_valid(model, row)
    size = @clp_ccall lengthNames Int32 (Ptr{Cvoid},) model.p
    row_name = Array{UInt8}(undef, size+1)
    @clp_ccall rowName Cvoid (Ptr{Cvoid}, Int32, Ptr{UInt8}) model.p (row-1) row_name
    return unsafe_string(row_name)
end

# Return an array with a column name.
# XXX deviation from Clp API syntax, which fills a user-provided
# array of chars:
#   void Clp_columnName(Clp_Simplex * model, int iColumn, char * name);
function column_name(model::ClpModel, col::Integer)
    _jl__check_model(model)
    _jl__check_col_is_valid(model, col)
    size = @clp_ccall lengthNames Int32 (Ptr{Cvoid},) model.p
    col_name = Array{UInt8}(undef, size+1)
    @clp_ccall columnName Cvoid (Ptr{Cvoid}, Int32, Ptr{UInt8}) model.p (col-1) col_name
    return unsafe_string(col_name)
end

# General solve algorithm which can do presolve.
function initial_solve(model::ClpModel)
    _jl__check_model(model)
    @clp_ccall initialSolve Int32 (Ptr{Cvoid},) model.p
end

function initial_solve_with_options(model::ClpModel,solve::ClpSolve)
    _jl__check_model(model)
    _jl__check_solve(solve)
    @clp_ccall initialSolveWithOptions Int32 (Ptr{Cvoid},Ptr{Cvoid}) model.p solve.p
end

initial_solve(model::ClpModel, solve::ClpSolve) = initial_solve_with_options(model,solve)

# Dual initial solve.
function initial_dual_solve(model::ClpModel)
    _jl__check_model(model)
    @clp_ccall initialDualSolve Int32 (Ptr{Cvoid},) model.p
end

# Primal initial solve.
function initial_primal_solve(model::ClpModel)
    _jl__check_model(model)
    @clp_ccall initialPrimalSolve Int32 (Ptr{Cvoid},) model.p
end

# Barrier initial solve.
function initial_barrier_solve(model::ClpModel)
    _jl__check_model(model)
    @clp_ccall initialBarrierSolve Int32 (Ptr{Cvoid},) model.p
end

# Barrier initial solve, no crossover.
function initial_barrier_no_cross_solve(model::ClpModel)
    _jl__check_model(model)
    @clp_ccall initialBarrierNoCrossSolve Int32 (Ptr{Cvoid},) model.p
end

# Dual algorithm.
function dual(model::ClpModel, ifValuesPass::Integer)
    _jl__check_model(model)
    @clp_ccall dual Int32 (Ptr{Cvoid},Int32) model.p ifValuesPass
end

# Primal algorithm.
function primal(model::ClpModel, ifValuesPass::Integer)
    _jl__check_model(model)
    @clp_ccall primal Int32 (Ptr{Cvoid},Int32) model.p ifValuesPass
end

# Solve the problem with the idiot code.
function idiot(model::ClpModel, tryhard::Integer)
    _jl__check_model(model)
    @clp_ccall idiot Int32 (Ptr{Cvoid},Int32) model.p tryhard
end

# Set or unset scaling:
#  0 - off
#  1 - equilibrium
#  2 - geometric
#  3 - auto
#  4 - dynamic (later)
function scaling(model::ClpModel, mode::Integer)
    _jl__check_model(model)
    if !(0 <= mode <= 4)
        error("Invalid clp scaling mode $mode (must be between 0 and 4)")
    end
    @clp_ccall scaling Cvoid (Ptr{Cvoid}, Int32) model.p mode
    return
end

# Get scaling flag.
function scaling_flag(model::ClpModel)
    _jl__check_model(model)
    return @clp_ccall scalingFlag Int32 (Ptr{Cvoid},) model.p
end

# Crash (at present just aimed at dual); returns:
#   -2 if dual preferred and crash basis created
#   -1 if dual preferred and all slack basis preferred
#   0 if basis going in was not all slack
#   1 if primal preferred and all slack basis preferred
#   2 if primal preferred and crash basis created.
#
#   If gap between bounds <="gap" variables can be flipped
#
#   If "pivot" is:
#     0 - No pivoting (so will just be choice of algorithm)
#     1 - Simple pivoting e.g. gub
#     2 - Mini iterations
function crash(model::ClpModel, gap::Float64, pivot::Int32)
    _jl__check_model(model)
    if !(0 <= pivot <= 2)
        error("Invalid clp crash pivot value $pivot (must be between 0 and 2)")
    end
    return @clp_ccall crash Int32 (Ptr{Cvoid}, Float64, Int32) model.p gap pivot
end

# Query whether the problem is primal feasible.
function primal_feasible(model::ClpModel)
    _jl__check_model(model)
    ret = @clp_ccall primalFeasible Int32 (Ptr{Cvoid},) model.p
    return ret != 0
end

# Query whether the problem is dual feasible.
function dual_feasible(model::ClpModel)
    _jl__check_model(model)
    ret = @clp_ccall dualFeasible Int32 (Ptr{Cvoid},) model.p
    return ret != 0
end

# Get dual bound.
function dual_bound(model::ClpModel)
    _jl__check_model(model)
    @clp_ccall dualBound Float64 (Ptr{Cvoid},) model.p
end

# Set dual bound.
function set_dual_bound(model::ClpModel, value::Real)
    _jl__check_model(model)
    @clp_ccall setDualBound Cvoid (Ptr{Cvoid}, Float64) model.p value
    return
end

# Get infeasibility cost.
function infeasibility_cost(model::ClpModel)
    _jl__check_model(model)
    @clp_ccall infeasibilityCost Float64 (Ptr{Cvoid},) model.p
end

# Set infeasibility cost.
function set_infeasibility_cost(model::ClpModel, value::Real)
    _jl__check_model(model)
    @clp_ccall setInfeasibilityCost Cvoid (Ptr{Cvoid}, Float64) model.p value
    return
end

# Get Perturbation. Values are:
#   50  - switch on perturbation
#   100 - auto perturb if takes too long (1.0e-6 largest nonzero)
#   101 - we are perturbed
#   102 - don't try perturbing again
# The default is 100; others are for playing.
function perturbation(model::ClpModel)
    _jl__check_model(model)
    @clp_ccall perturbation Int32 (Ptr{Cvoid},) model.p
end

# Set Perturbation.
function set_perturbation(model::ClpModel, value::Integer)
    _jl__check_model(model)
    if !(value == 50 || value == 100 || value == 101 || value == 102)
        error("Invalid clp perturbation value: $value (must be one of 50,100,101,102)")
    end
    @clp_ccall setPerturbation Cvoid (Ptr{Cvoid}, Int32) model.p value
    return
end

# Get current (or last) algorithm.
function algorithm(model::ClpModel)
    _jl__check_model(model)
    @clp_ccall algorithm Int32 (Ptr{Cvoid},) model.p
end

# Set algorithm.
function set_algorithm(model::ClpModel, value::Integer)
    _jl__check_model(model)
    # XXX which values of the algorithm are valid ???
    @clp_ccall setAlgorithm Cvoid (Ptr{Cvoid}, Int32) model.p value
    return
end

# Get the sum of dual infeasibilities.
function sum_dual_infeasibilities(model::ClpModel)
    _jl__check_model(model)
    @clp_ccall sumDualInfeasibilities Float64 (Ptr{Cvoid},) model.p
end

# Get the number of dual infeasibilities.
function number_dual_infeasibilities(model::ClpModel)
    _jl__check_model(model)
    @clp_ccall numberDualInfeasibilities Int32 (Ptr{Cvoid},) model.p
end

# Get the sum of primal infeasibilities.
function sum_primal_infeasibilities(model::ClpModel)
    _jl__check_model(model)
    @clp_ccall sumPrimalInfeasibilities Float64 (Ptr{Cvoid},) model.p
end

# Get the number of primal infeasibilities.
function number_primal_infeasibilities(model::ClpModel)
    _jl__check_model(model)
    @clp_ccall numberPrimalInfeasibilities Int32 (Ptr{Cvoid},) model.p
end

# Save model to file, returns 0 if success.  This is designed for
# use outside algorithms so does not save iterating arrays etc.
# It does not save any messaging information.
# Does not save scaling values.
# It does not know about all types of virtual functions.
function save_model(model::ClpModel, file_name::String)
    _jl__check_model(model)
    @assert isascii(file_name)

    @clp_ccall saveModel Int32 (Ptr{Cvoid}, Cstring) model.p file_name
end

# Restore model from file, returns 0 if success,
# deletes current model.
function restore_model(model::ClpModel, file_name::String)
    _jl__check_model(model)
    @assert isascii(file_name)

    @clp_ccall restoreModel Int32 (Ptr{Cvoid}, Cstring) model.p file_name
end

# Just check solution (for external use) - sets sum of
# infeasibilities etc.
function check_solution(model::ClpModel);
    _jl__check_model(model)
    @clp_ccall checkSolution Cvoid (Ptr{Cvoid},) model.p
    return
end

# Query whether there are numerical difficulties.
function is_abandoned(model::ClpModel)
    _jl__check_model(model)
    ret = @clp_ccall isAbandoned Int32 (Ptr{Cvoid},) model.p
    return ret != 0
end

# Query whether optimality is proven.
function is_proven_optimal(model::ClpModel)
    _jl__check_model(model)
    ret = @clp_ccall isProvenOptimal Int32 (Ptr{Cvoid},) model.p
    return ret != 0
end

# Query whether primal infeasiblity is proven.
function is_proven_primal_infeasible(model::ClpModel)
    _jl__check_model(model)
    ret = @clp_ccall isProvenPrimalInfeasible Int32 (Ptr{Cvoid},) model.p
    return ret != 0
end

# Query whether dual infeasiblity is proven.
function is_proven_dual_infeasible(model::ClpModel)
    _jl__check_model(model)
    ret = @clp_ccall isProvenDualInfeasible Int32 (Ptr{Cvoid},) model.p
    return ret != 0
end

# Query whether the given primal objective limit is reached.
function is_primal_objective_limit_reached(model::ClpModel)
    _jl__check_model(model)
    ret = @clp_ccall isPrimalObjectiveLimitReached Int32 (Ptr{Cvoid},) model.p
    return ret != 0
end

# Query whether the given dual objective limit is reached.
function is_dual_objective_limit_reached(model::ClpModel)
    _jl__check_model(model)
    ret = @clp_ccall isDualObjectiveLimitReached Int32 (Ptr{Cvoid},) model.p
    return ret != 0
end

# Query whether the iteration limit is reached
function is_iteration_limit_reached(model::ClpModel)
    _jl__check_model(model)
    ret = @clp_ccall isIterationLimitReached Int32 (Ptr{Cvoid},) model.p
    return ret != 0
end

# Get the direction of optimization:
#   1  - minimize
#   -1 - maximize
#   0  - ignore
function get_obj_sense(model::ClpModel)
    _jl__check_model(model)
    @clp_ccall getObjSense Float64 (Ptr{Cvoid},) model.p
end

# Set the direction of optimization.
function set_obj_sense(model::ClpModel, objsen::Real)
    _jl__check_model(model)
    if !(objsen == -1 || objsen == 0 || objsen == 1)
        error("Invalid clp objsense $objsense (should be -1,0 or 1)")
    end
    @clp_ccall setObjSense Cvoid (Ptr{Cvoid}, Float64) model.p objsen
    return
end

# Print model for debugging purposes
function print_model(model::ClpModel, prefix::String)
    _jl__check_model(model)
    @assert isascii(prefix)

    @clp_ccall printModel Cvoid (Ptr{Cvoid}, Cstring) model.p prefix
end

function get_small_element_value(model::ClpModel)
    _jl__check_model(model)

    @clp_ccall getSmallElementValue Float64 (Ptr{Cvoid},) model.p
end

function set_small_element_value(model::ClpModel,value::Float64)
    _jl__check_model(model)

    @clp_ccall setSmallElementValue Cvoid (Ptr{Cvoid},Float64) model.p value
end

# ClpSolve functions

function set_special_option(solve::ClpSolve, which::Integer, value::Integer, extraInfo::Integer)
    _jl__check_solve(solve)

    @clpsolve_ccall setSpecialOption Cvoid (Ptr{Cvoid},Int32,Int32,Int32) solve.p which value extraInfo
end

function get_special_option(solve::ClpSolve, which::Integer)
    _jl__check_solve(solve)

    @clpsolve_ccall getSpecialOption Int32 (Ptr{Cvoid},Int32) solve.p which
end

macro def_get_int_property(fname,clpname)
    quote
        function $(esc(fname))(solve::ClpSolve)
            _jl__check_solve(solve)
            @clpsolve_ccall $clpname Int32 (Ptr{Cvoid},) solve.p
        end
    end
end

macro def_set_int_property(fname,clpname)
    quote
        function $(esc(fname))(solve::ClpSolve, val::Integer)
            _jl__check_solve(solve)
            @clpsolve_ccall $clpname Cvoid (Ptr{Cvoid},Int32) solve.p val
        end
    end
end

# 0 - presolve on
# 1 - presolve off
# 2 - presolve number
# 3 - presolve number cost
function set_presolve_type(solve::ClpSolve, value::Integer)
    _jl__check_solve(solve)

    @clpsolve_ccall setPresolveType Cvoid (Ptr{Cvoid},Int32,Int32) solve.p value -1
end

@def_get_int_property get_presolve_type getPresolveType

# 0 - dual simplex
# 1 - primal simplex
# 2 - primal or sprint
# 3 - barrier
# 4 - barrier no crossover
# 5 - automatic
function set_solve_type(solve::ClpSolve, value::Integer)
    _jl__check_solve(solve)

    @clpsolve_ccall setSolveType Cvoid (Ptr{Cvoid},Int32,Int32) solve.p value -1
end

@def_get_int_property get_solve_type getSolveType

@def_get_int_property get_presolve_passes getPresolvePasses

function get_extra_info(solve::ClpSolve, which::Integer)
    _jl__check_solve(solve)

    @clpsolve_ccall getExtraInfo Int32 (Ptr{Cvoid},Int32) solve.p which
end

@def_set_int_property set_infeasible_return setInfeasibleReturn
@def_get_int_property infeasible_return infeasibleReturn

@def_get_int_property do_dual doDual
@def_set_int_property set_do_dual setDoDual

@def_get_int_property do_singleton doSingleton
@def_set_int_property set_do_singleton setDoSingleton

@def_get_int_property do_doubleton doDoubleton
@def_set_int_property set_do_doubleton setDoDoubleton

@def_get_int_property do_tripleton doTripleton
@def_set_int_property set_do_tripleton setDoTripleton

@def_get_int_property do_tighten doTighten
@def_set_int_property set_do_tighten setDoTighten

@def_get_int_property do_forcing doForcing
@def_set_int_property set_do_forcing setDoForcing

@def_get_int_property do_implied_free doImpliedFree
@def_set_int_property set_do_implied_free setDoImpliedFree

@def_get_int_property do_dupcol doDupcol
@def_set_int_property set_do_dupcol setDoDupcol

@def_get_int_property do_duprow doDuprow
@def_set_int_property set_do_duprow setDoDuprow

@def_get_int_property do_singleton_column doSingletonColumn
@def_set_int_property set_do_singleton_column setDoSingletonColumn

@def_get_int_property presolve_actions presolveActions
@def_set_int_property set_presolve_actions setPresolveActions

@def_get_int_property substitution substitution
@def_set_int_property set_substitution setSubstitution

end


#This function will actually constrain the model based on deleted genes

function deleteModelGenes(data_dictionary, drGenes)
genes = deepcopy(data_dictionary["list_of_gene_symbols"]) #need to update this later
genes_minus_deletions = [] #our genelist will eventually be marked
rxnGeneMat = data_dictionary["rxnGeneMat"] #need this for later
flux_bounds_array = deepcopy(data_dictionary["flux_bounds_array"]) #will need to constrain certain fluxes
rules = deepcopy(data_dictionary["irules"])
hasEffect = false #flag to let us know if something actually changes
constrainRxnNames = []

for i = 1:length(genes)
    push!(genes_minus_deletions, replace(genes[i], "_deleted" => ""))
end

#Check if there's genes that don't exist in the deleted genes list
geneInd = indexin(drGenes, genes_minus_deletions)
flag = false
for i = 1:length(geneInd)
    if geneInd[i] == nothing
        flag = true
    end
end
#println(flag)

if ~flag #if our list of deleted genes is in the model, proceed
deletedGeneNames = []
    for i in geneInd
        append!(deletedGeneNames, genes[i])
        genes[i] = genes_minus_deletions[i]*"_deleted" #marks the genes for deletion
    end

    #lets grab our list of rxns that are associated with our deleted genes
    rxnInd = []
    for i in geneInd
        append!(rxnInd, findall(rxnGeneMat[:,i] .== 1))
    end # now rxnInd inludes rxns that are affected by the deleted genes

    if ~isempty(rxnInd) #if affected rxns
        x = true.*ones(length(genes)) #represent genes as booleans
        for i in geneInd #set all of the deleted genes to false for the boolean
            x[i] = false
        end

        x = x .== 1 #convert x to a bit array that can be evaluated as a boolean
        constrainRxns = false.*ones(length(rxnInd))
        evaluatedRules = calculate_gene_rules_vector(data_dictionary, x)
        evaluatedRules = evaluatedRules .== 1
        #now it's time to figure out which rxns are constrained
        for j = 1:length(rxnInd)
            if ~isempty(evaluatedRules[rxnInd[j]]) #if the rules are empty, theres no regulation
                if ~evaluatedRules[rxnInd[j]] #if the rule eval turns out false..
                    constrainRxns[j] = true #this rxn will be constrained
                end
            end
        end
        if any(constrainRxns .== true) #if we have constrainedRxns
            for k = 1:length(constrainRxns)
                if constrainRxns[k] == true #if the particular index is true
                    push!(constrainRxnNames, data_dictionary["list_of_reaction_name_strings"][rxnInd[k]])
                    flux_bounds_array[rxnInd[k],:] .= 0 #silence by setting flux to 0
                end
            end
            hasEffect = true
        end
    end

    #output updated gene list, flux bounds,
    deleted_data_dictionary = deepcopy(data_dictionary)
    deleted_data_dictionary["list_of_gene_symbols"] = genes
    deleted_data_dictionary["flux_bounds_array"] = flux_bounds_array
    trueconstrainRxnNames = unique(constrainRxnNames)
    return (deleted_data_dictionary, hasEffect, trueconstrainRxnNames, deletedGeneNames)

else
    error("a gene is not in the model!")
end
end

#function buildRxnGeneMat(data_dictionary)
#rxnGeneMat = false.*ones(data_dictionary["number_of_reactions"], length(data_dictionary["list_of_gene_symbols"])

#end


"""
    RexHopper <: Environment

    hopping robot designed and build by the Robotic Exploration Lab (Benjamin Bokser)
"""
struct RexHopper end

function rexhopper(;
    representation=:minimal,
    model=:rexhopper,
    timestep=0.05,
    seed=1,
    gravity=[0.0; 0.0; -9.81],
    friction_coefficient=1.0,
    spring=0.0,
    damper=1.0,
    contact_type=:nonlinear,
    contact_foot=true,
    contact_body=true,
    limits=true,
    info=nothing,
    infeasible_control=false,
    vis=Visualizer(),
    name=:robot,
    opts_step=SolverOptions(rtol=1.0e-4, btol=1.0e-4, undercut=10.0),
    opts_grad=SolverOptions(rtol=1.0e-4, btol=1.0e-4, undercut=10.0),
    T=Float64)

    mechanism = get_rexhopper(
        model=model,
        limits=limits,
        contact_type=contact_type,
        timestep=timestep,
        gravity=gravity,
        friction_coefficient=friction_coefficient,
        spring=spring,
        damper=damper,
        contact_foot=contact_foot,
        contact_body=contact_body)

    initialize_rexhopper!(mechanism)

    if representation == :minimal
        nx = minimal_dimension(mechanism)
    elseif representation == :maximal
        nx = maximal_dimension(mechanism)
    end
    nu_inf = input_dimension(mechanism)
    nu = infeasible_control ? nu_inf : 5
    no = nx

    # values taken from Mujoco's model, combining the control range -1, 1 and the motor gears.
    aspace = BoxSpace(nu,
        low=(-Inf * ones(nu)),
        high=(Inf * ones(nu)))
    ospace = BoxSpace(no,
        low=(-Inf * ones(no)),
        high=(Inf * ones(no)))

    rng = MersenneTwister(seed)

    z = get_maximal_state(mechanism)
    x = representation == :minimal ? maximal_to_minimal(mechanism, z) : z

    fx = zeros(nx, nx)
    fu = zeros(nx, nu)

    u_prev = zeros(nu)
    control_map = infeasible_control ? 1.0 * I(nu) : cat(zeros(3, 3), 1.0 * I(3), 1.0, 0.0, 1.0, zeros(5, 5), dims=(1,2))

    build_robot(mechanism, vis=vis, name=name)

    TYPES = [RexHopper, T, typeof(mechanism), typeof(aspace), typeof(ospace), typeof(info)]
    env = Environment{TYPES...}(mechanism,
        representation,
        aspace,
        ospace,
        x, fx, fu,
        u_prev, control_map,
        nx, nu, no,
        info,
        [rng], vis,
        opts_step, opts_grad)

    return env
end

function Base.reset(env::Environment{RexHopper};
    x=nothing)

    if x != nothing
        env.state .= x
    else
        # initialize above the ground to make sure that with random initialization we do not violate the ground constraint.
        initialize!(env.mechanism, :rexhopper)
        x = get_minimal_state(env.mechanism)
        nx = minimal_dimension(env.mechanism)
        z = minimal_to_maximal(env.mechanism, x)
        set_maximal_state!(env.mechanism, z)
        if env.representation == :minimal
            env.state .= get_minimal_state(env.mechanism)
        elseif env.representation == :maximal
            env.state .= get_maximal_state(env.mechanism)
        end
        env.input_previous .= 0.0
    end
    return get_observation(env)
end


export RoyaltyModelNoHet, RoyaltyModel, ObservationRoyalty, DataRoyalty,
    observed_choices

# Model structure
#---------------------------

"Royalty rates"
struct RoyaltyModelNoHet <: AbstractRoyaltyModel end
struct RoyaltyModel      <: AbstractRoyaltyModel end

# Abstract data strucutres
#---------------------------

struct ObservationRoyalty{M<:AbstractRoyaltyModel,I<:Integer, T<:Real, V<:AbstractVector{T}} <: AbstractObservation
    model::M
    y::I
    x::V
    xbeta::T
    num_choices::I
    function ObservationRoyalty(model::M,y::I,x::V,xbeta::T,num_choices::I) where {M<:AbstractRoyaltyModel,I<:Integer, T<:Real, V<:AbstractVector{T}}
        isfinite(xbeta) || throw(DomainError(xbeta, "$xbeta = xβ = $x * β not finite")) # FIXME
        0 < y <= num_choices || throw(DomainError(y, "y = $y should be 1 <= y <= $num_choices"))
        return new{M,I,T,V}(model, y, x, xbeta, num_choices)
    end
end

struct DataRoyalty{M<:AbstractRoyaltyModel,I<:Integer,AV<:AbstractVector, T<:Real} <: AbstractDataSet
    model::M
    y::Vector{I}
    x::Matrix{T}
    xbeta::Vector{T}
    choices::AV
    function DataRoyalty(model::M, y::Vector{I}, x::Matrix{T},choices::AV) where {M,I,AV,T}
        k,n = size(x)
        length(y) == n  || throw(DimensionMismatch())
        issorted(choices) || throw(error("choices not sorted"))
        L = length(choices)
        extrema(y) == (1,L) || throw(error("lowest royalty ID > 1"))
        L == length(countmap(y)) || throw(error("highest royalty ID < L = $L"))
        return new{M,I,AV,T}(model, y, x, Vector{T}(undef, n), choices)
    end
end

const DataOrObsRoyalty = Union{ObservationRoyalty{M},DataRoyalty{M}} where {M}
const ObservationGroupRoyalty = ObservationGroup{<:DataRoyalty}
const AbstractDataStructureRoyalty = Union{ObservationRoyalty,DataRoyalty,ObservationGroupRoyalty}

function DataRoyalty(m::AbstractRoyaltyModel, d::DataRoyalty)
    DataRoyalty(m, _y(d), _x(d), choices(d))
end

# Common interfaces
#---------------------------

_xbeta(      d::DataOrObsRoyalty) = d.xbeta
choices(d::DataRoyalty) = d.choices
num_choices(d::ObservationRoyalty) = d.num_choices
num_choices(d::DataRoyalty) = length(choices(d))

observed_choices(d::DataRoyalty) = getindex(choices(d), _y(d))

@deprecate _num_choices(d::DataOrObsRoyalty) num_choices(d)

# DataRoyalty interface
#---------------------------

length(   d::DataRoyalty) = length(_y(d))
group_ptr(d::DataRoyalty) = OneTo(length(d)+1)
obs_ptr(  d::DataRoyalty) = OneTo(length(d)+1)

function choice_in_model(d::DataOrObsRoyalty, l::Integer)
    0 < l <= num_choices(d) && return true
    throw(DomainError(l, "$l outside of 1:$(num_choices(d))"))
end

function choice_in_model(d, ls)
    for l in ls
        choice_in_model(d,l)
    end
    return nothing
end

# Iteration over data
#---------------------------

function Observation(d::DataRoyalty, k::Integer)
    k ∈ eachindex(d) || throw(BoundsError(d,k))
    y = getindex(_y(d), k)
    x = view(_x(d), :, k)
    xbeta = getindex(_xbeta(d), k)
    return ObservationRoyalty(_model(d), y, x, xbeta, num_choices(d))
end

getindex(   g::ObservationGroupRoyalty, k) = Observation(_data(g), getindex(grouprange(g), k))
Observation(g::ObservationGroupRoyalty, k) = getindex(g,k)

# Observation
#---------------------------

function ==(a::ObservationRoyalty, b::ObservationRoyalty)
    _y(a) == _y(b) &&
    _x(a) == _x(b) &&
    _xbeta(a) == _xbeta(b) &&
    num_choices(a) == num_choices(b)
end

# Updating data
#---------------------------

function update_xbeta!(d::DataRoyalty, theta::AbstractVector)
    # length(theta) == _num_x(d) || throw(DimensionMismatch("theta: $(size(theta)) and x: $(size(_x(d)))"))
    mul!(_xbeta(d), _x(d)', theta)
    return nothing
end

update!(d::DataRoyalty, theta) = update_xbeta!(d,theta_royalty_β(d,theta))

# Parameter interfaces
#---------------------------

# length of RoyaltyModel parameters & gradient
extra_parm(::RoyaltyModel) = 2
extra_parm(::RoyaltyModelNoHet) = 0
extra_parm(d::DataOrObsRoyalty{<:AbstractRoyaltyModel}) = extra_parm(_model(d))

_nparm(d::DataOrObsRoyalty) = _num_x(d) + num_choices(d) - 1 + extra_parm(d)

idx_royalty(d::Union{DataOrObsRoyalty,AbstractRoyaltyModel}) = OneTo(_nparm(d))
theta_royalty(d, theta) = view(theta, idx_royalty(d))

# parameter vector
idx_royalty_ρ(d::Union{DataOrObsRoyalty{<:RoyaltyModelNoHet},RoyaltyModelNoHet}) = 1:0
idx_royalty_ψ(d::Union{DataOrObsRoyalty{<:RoyaltyModelNoHet},RoyaltyModelNoHet}) = 1:0
idx_royalty_β(d::DataOrObsRoyalty{<:RoyaltyModelNoHet}) = (1:_num_x(d))
idx_royalty_κ(d::DataOrObsRoyalty{<:RoyaltyModelNoHet}) = _num_x(d) .+ (1:num_choices(d)-1)

function idx_royalty_κ(d::DataOrObsRoyalty{RoyaltyModelNoHet}, l)
    choice_in_model(d,l)
    return _num_x(d) .+ l
end

idx_royalty_ρ(d::Union{DataOrObsRoyalty{RoyaltyModel},RoyaltyModel}) = 1
idx_royalty_ψ(d::Union{DataOrObsRoyalty{RoyaltyModel},RoyaltyModel}) = 2
idx_royalty_β(d::DataOrObsRoyalty{RoyaltyModel}) = 2 .+ (1:_num_x(d))
idx_royalty_κ(d::DataOrObsRoyalty{RoyaltyModel}) = 2 + _num_x(d) .+ (1:num_choices(d)-1)
function idx_royalty_κ(d::DataOrObsRoyalty{RoyaltyModel}, l::Integer)
    choice_in_model(d,l)
    return 2 + _num_x(d) + l
end
function idx_royalty_κ(d::DataOrObsRoyalty{RoyaltyModel}, ls::AbstractVector)
    choice_in_model(d,ls)
    return (2 + _num_x(d)) .+ ls
end

# get coefs
theta_royalty_ρ(d, theta) = theta[idx_royalty_ρ(d)]
theta_royalty_ψ(d, theta) = theta[idx_royalty_ψ(d)]
theta_royalty_β(d, theta) = view(theta, idx_royalty_β(d))
theta_royalty_κ(d, theta) = view(theta, idx_royalty_κ(d))
theta_royalty_κ(d, theta, l::Integer) = theta[idx_royalty_κ(d,l)]
theta_royalty_κ(d, theta, ls) = view(theta, idx_royalty_κ(d,ls))

kappa_level_to_cumsum(x) = vcat(first(x), sqrt.(2 .* diff(x)) )
kappa_cumsum_to_level(x) = first(x) .+ vcat(0, 0.5 .* cumsum(x[2:end].^2))
function update_kappa_level_to_cumsum!(x)
    x .= kappa_level_to_cumsum(x)
end

function theta_royalty_level_to_cumsum(d,x)
    xnew = copy(x)
    kap = theta_royalty_κ(d,x)
    xnew[idx_royalty_κ(d)] .= kappa_level_to_cumsum(kap)
    return xnew
end
function theta_royalty_cumsum_to_level(d,x)
    xnew = copy(x)
    kap = theta_royalty_κ(d,x)
    xnew[idx_royalty_κ(d)] .= kappa_cumsum_to_level(kap)
    return xnew
end

# @deprecate theta_roy(d, theta) theta_royalty(d,theta)

# check if theta is okay
theta_royalty_check(d, theta) = issorted(theta_royalty_κ(d,theta))

function check_theta(d::DataOrObsRoyalty, theta)
    _nparm(d) == length(theta) || throw(DimensionMismatch("_nparm(d) = $(_nparm(d)) != length(theta) = $(length(theta))"))
    all(isfinite.(theta)) || throw(error("theta = $theta not finite"))
    kappa = theta_royalty_κ(d,theta)
    # issorted(kappa) || @warn "kappa = $kappa not sorted"
    return true
end

function coefnames(d::DataRoyalty)
    nms = Vector{String}(undef, _nparm(d))
    if _model(d) isa RoyaltyModel
        nms[idx_royalty_ρ(d)] = "\\rho"
        nms[idx_royalty_ψ(d)] = "\\psi^0"
    end
    for (i,nmi) in enumerate(idx_royalty_β(d))
        nms[nmi] = "\\beta_$i"
    end
    for (i, nmi) in enumerate(idx_royalty_κ(d))
        nms[nmi] = "\\kappa_$i"
    end
    return nms
end

"""
    DataRoyalty(u,v,X,theta,L)

Simulate dataset for `RoyaltyModel` using `u,v` to make `ψ1`
"""
function DataRoyalty(u::AbstractVector, v::AbstractVector, X::Matrix, theta::Vector, rates::AbstractVector)

    k,nobs = size(X)
    L = length(rates)
    L >= 3 || throw(error("L = $L !>= 3"))

    k == length(theta) - (L-1) - 2 || throw(DimensionMismatch("dim mismatch b/w X: $(size(X)) and θ = $theta"))
    k >= 1 || throw(error("theta too short"))
    nobs == length(u) == length(v) || throw(DimensionMismatch())

    # get ψ1
    ψ1 = similar(u)
    dψ1dρ = similar(u)
    update_ψ1!(ψ1, u, v, first(theta))
    update_dψ1dθρ!(dψ1dρ, u, v, first(theta))

    eps    = randn(nobs)

    rstar  = theta[2] .* ψ1 .+ X'*theta[2 .+ (1:k)] .+ eps
    cutoffs = kappa_cumsum_to_level(theta[end-L+2:end])
    l = map((r) ->  searchsortedfirst(cutoffs, r), rstar)
    data = DataRoyalty(RoyaltyModel(),l,X,rates)

    return data
end

function DataRoyalty(u, v, X, theta, L::Integer=3)
    if L == 3
        rates = [1/8, 3/16, 1/4]
    else
        throw(error("L != 3"))
    end
    return DataRoyalty(u,v,X,theta,rates)
end

"""
    DataRoyalty(u,v,theta,L)

Simulate dataset for `RoyaltyModel` generating random X
"""
function DataRoyalty(u::AbstractVector, v::AbstractVector, theta::Vector, L::Integer=3)
    k = length(theta) - (L-1) - 2
    nobs = length(u)
    X = randn(k, nobs)
    return DataRoyalty(u,v,X,theta,L)
end

function DataRoyalty(num_i::Integer, theta::Vector, L::Integer=3)
    return DataRoyalty(randn(num_i), randn(num_i), theta, L)
end


module AtomIO
using AtomsBase
using PyCall
using Unitful

export load_system, save_system

function load_system(file::AbstractString)
    ase = pyimport_e("ase")
    ispynull(ase) && error("Install ASE to load data from exteral files")
    ase_atoms = pyimport("ase.io").read(file)

    T = Float64
    cell_julia = convert(Array, ase_atoms.cell)  # Array of arrays
    box = [[T(cell_julia[j][i]) * u"Å" for i = 1:3] for j = 1:3]

    atoms = map(ase_atoms) do at
        atnum = convert(Int, at.number)
        Atom(atnum, at.position * u"Å"; magnetic_moment=at.magmom)
    end
    bcs = [p ? Periodic() : DirichletZero() for p in ase_atoms.pbc]
    atomic_system(atoms, box, bcs)
end

function save_system(filename::AbstractString, system::AbstractSystem)
    error("Not yet implemented.")
end

end


module IOModule

using NetCDF
using NetCDF: NcFile,NcVar,NC_NETCDF4
using GridModule: Grid, LatLonGrid, UnstructuredGrid, get_number_of_dimensions
using FieldModule: Field, LatLonField,UnstructuredField,LatLonDirectionIndicators
using FieldModule: UnstructuredDirectionIndicators
using FieldModule: round,convert,invert,add_offset,get_data,maximum,equals
using HDModule: RiverParameters,RiverPrognosticFields
using LakeModule: LakeParameters,LatLonLakeParameters, UnstructuredLakeParameters
using LakeModule: GridSpecificLakeParameters,LakeFields
using MergeTypesModule

import LakeModule: write_lake_numbers_field,write_lake_volumes_field
import LakeModule: write_diagnostic_lake_volumes_field
import HDModule: write_river_initial_values,write_river_flow_field

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

function get_additional_grid_information(file_name::AbstractString)
  clat::Array{Float64,1} = NetCDF.ncread(file_name,"clat")
  clon::Array{Float64,1} = NetCDF.ncread(file_name,"clon")
  clat_bounds::Array{Float64,2} = NetCDF.ncread(file_name,"clat_bnds")
  clon_bounds::Array{Float64,2} = NetCDF.ncread(file_name,"clon_bnds")
  return clat,clon,clat_bounds,clon_bounds
end

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

function load_3d_field(file_handle::NcFile,grid::Grid,variable_name::AbstractString,
                       field_type::DataType,layer=Int64,;timestep::Int64=-1)
  variable::NcVar = file_handle[variable_name]
  values::Array{field_type,(timestep == -1) ?
                            get_number_of_dimensions(grid)+1 :
                            get_number_of_dimensions(grid)+2 } = NetCDF.readvar(variable)
  local layer_values::Array{field_type,get_number_of_dimensions(grid)}
  if isa(grid,UnstructuredGrid)
    if timestep == -1
      layer_values = values[:,layer]
    else
      layer_values = values[:,layer,timestep]
    end
  end
  return Field{field_type}(grid,layer_values)
end

function load_array_of_fields(file_handle::NcFile,grid::Grid,variable_base_name::AbstractString,
                              field_type::DataType,number_of_fields::Int64)
  if number_of_fields == 1 && isa(grid,LatLonGrid)
    return Field{field_type}[load_field(file_handle,grid,variable_base_name,field_type)]
  else
    array_of_fields::Array{Field{field_type},1} = Array{Field{field_type}}(undef,number_of_fields)
    for i = 1:number_of_fields
      if isa(grid,LatLonGrid)
        array_of_fields[i] = load_field(file_handle,grid,variable_base_name*string(i),field_type)
      else
        array_of_fields[i] = load_3d_field(file_handle,grid,variable_base_name,field_type,i)
      end
    end
    return array_of_fields
  end
end

function prep_array_of_fields(dims::Array{NcDim},
                              variable_base_name::AbstractString,
                              array_of_fields::Array{Field{T},1},
                              fields_to_write::Vector{Pair}) where {T}
  number_of_fields::Int64 = size(array_of_fields,1)
  if number_of_fields == 1
    return prep_field(dims,variable_base_name,array_of_fields[1],fields_to_write)
  else
    for i = 1:number_of_fields
      prep_field(dims,variable_base_name*string(i),
                 array_of_fields[i],fields_to_write)
    end
  end
end

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

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

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

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

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

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

function load_river_parameters(hd_para_filepath::AbstractString,grid::Grid;
                               day_length=86400.0,step_length=86400.0)
  println("Loading: " * hd_para_filepath)
  file_handle::NcFile = NetCDF.open(hd_para_filepath)
  local landsea_mask::Field{Bool}
  try
    if isa(grid,LatLonGrid)
      direction_based_river_directions = load_field(file_handle,grid,"FDIR",Float64)
      flow_directions = LatLonDirectionIndicators(round(Int64,direction_based_river_directions))
      landsea_mask =
        load_field(file_handle,grid,"FLAG",Bool)
    elseif isa(grid,UnstructuredGrid)
      index_based_river_directions = load_field(file_handle,grid,"FDIR",Float64)
      flow_directions = UnstructuredDirectionIndicators(round(Int64,index_based_river_directions))
      landsea_mask_int::Field{Int64} =
        load_field(file_handle,grid,"MASK",Int64)
      landsea_mask = equals(landsea_mask_int,1)
    end
    landsea_mask = invert(landsea_mask)
    river_reservoir_nums::Field{Int64} =
      round(Int64,load_field(file_handle,grid,"ARF_N",Float64))
    overland_reservoir_nums::Field{Int64} =
      round(Int64,load_field(file_handle,grid,"ALF_N",Float64))
    base_reservoir_nums::Field{Int64} = Field{Int64}(grid,1)
    river_retention_coefficients::Field{Float64} =
      load_field(file_handle,grid,"ARF_K",Float64)
    overland_retention_coefficients::Field{Float64} =
      load_field(file_handle,grid,"ALF_K",Float64)
    base_retention_coefficients::Field{Float64} =
      load_field(file_handle,grid,"AGF_K",Float64)
    return  RiverParameters(flow_directions,
                            river_reservoir_nums,
                            overland_reservoir_nums,
                            base_reservoir_nums,
                            river_retention_coefficients,
                            overland_retention_coefficients,
                            base_retention_coefficients,
                            landsea_mask,grid,day_length,
                            step_length)
  finally
    NetCDF.close(file_handle)
  end
end

function load_river_initial_values(hd_start_filepath::AbstractString,grid::Grid,
                                   river_parameters::RiverParameters)
  river_prognostic_fields::RiverPrognosticFields =
    RiverPrognosticFields(river_parameters)
  local reservoir_units_adjustment_factor
  println("Loading: " * hd_start_filepath)
  file_handle::NcFile = NetCDF.open(hd_start_filepath)
  try
    if ! isa(grid,UnstructuredGrid)
      reservoir_units_adjustment = 1.0/river_parameters.step_length
      river_prognostic_fields.river_inflow =
        load_field(file_handle,river_parameters.grid,"FINFL",Float64)
    else
      river_prognostic_fields.river_inflow = Field{Float64}(grid,0.0)
      reservoir_units_adjustment = 1.0
    end
    river_prognostic_fields.base_flow_reservoirs =
      load_array_of_fields(file_handle,river_parameters.grid,"FGMEM",Float64,
                            maximum(river_parameters.base_reservoir_nums))
    river_prognostic_fields.base_flow_reservoirs *= reservoir_units_adjustment
    river_prognostic_fields.overland_flow_reservoirs =
      load_array_of_fields(file_handle,river_parameters.grid,"FLFMEM",Float64,
                            maximum(river_parameters.overland_reservoir_nums))
    river_prognostic_fields.overland_flow_reservoirs *= reservoir_units_adjustment
    river_prognostic_fields.river_flow_reservoirs =
      load_array_of_fields(file_handle,river_parameters.grid,"FRFMEM",Float64,
                            maximum(river_parameters.river_reservoir_nums))
    river_prognostic_fields.river_flow_reservoirs *= reservoir_units_adjustment
  finally
    NetCDF.close(file_handle)
  end
  return river_prognostic_fields
end

function write_river_initial_values(hd_start_filepath::AbstractString,
                                    river_parameters::RiverParameters,
                                    river_prognostic_fields::RiverPrognosticFields)
  println("Writing: " * hd_start_filepath)
  if ! isa(river_parameters.grid,UnstructuredGrid)
    reservoir_units_adjustment = river_parameters.step_length
  else
    reservoir_units_adjustment = 1.0
  end
  river_prognostic_fields.base_flow_reservoirs *= reservoir_units_adjustment
  river_prognostic_fields.overland_flow_reservoirs *= reservoir_units_adjustment
  river_prognostic_fields.river_flow_reservoirs *= reservoir_units_adjustment
  fields_to_write::Vector{Pair} = Pair[]
  dims::Array{NcDim} = prep_dims(river_parameters.grid)
  prep_field(dims,"FINFL",
             river_prognostic_fields.river_inflow,fields_to_write)
  prep_array_of_fields(dims,"FGMEM",
                       river_prognostic_fields.base_flow_reservoirs,
                       fields_to_write)
  prep_array_of_fields(dims,"FLFMEM",
                       river_prognostic_fields.overland_flow_reservoirs,
                       fields_to_write)
  prep_array_of_fields(dims,"FRFMEM",
                       river_prognostic_fields.river_flow_reservoirs,
                       fields_to_write)
  write_fields(river_parameters.grid,fields_to_write,hd_start_filepath,dims)
end

function load_lake_parameters(lake_para_filepath::AbstractString,grid::Grid,hd_grid::Grid,
                              surface_model_grid::Grid)
  println("Loading: " * lake_para_filepath)
  file_handle::NcFile = NetCDF.open(lake_para_filepath)
  try
    grid_specific_lake_parameters::GridSpecificLakeParameters =
      load_grid_specific_lake_parameters(file_handle,grid,surface_model_grid)
    lake_centers::Field{Bool} =
      load_field(file_handle,grid,"lake_centers",Bool)
    connection_volume_thresholds::Field{Float64} =
      load_field(file_handle,grid,"connection_volume_thresholds",Float64)
    flood_volume_thresholds::Field{Float64} =
      load_field(file_handle,grid,"flood_volume_thresholds",Float64)
    flood_local_redirect::Field{Bool} =
      load_field(file_handle,grid,"flood_local_redirect",Bool)
    connect_local_redirect::Field{Bool} =
      load_field(file_handle,grid,"connect_local_redirect",Bool)
    additional_flood_local_redirect::Field{Bool} =
      load_field(file_handle,grid,"additional_flood_local_redirect",Bool)
    additional_connect_local_redirect::Field{Bool} =
      load_field(file_handle,grid,"additional_connect_local_redirect",Bool)
    merge_points::Field{MergeTypes} =
      load_field(file_handle,grid,"merge_points",Int64)
    return LakeParameters(lake_centers,
                          connection_volume_thresholds,
                          flood_volume_thresholds,
                          flood_local_redirect,
                          connect_local_redirect,
                          additional_flood_local_redirect,
                          additional_connect_local_redirect,
                          merge_points,grid,hd_grid,
                          surface_model_grid,
                          grid_specific_lake_parameters)
  finally
    NetCDF.close(file_handle)
  end
end

function write_river_flow_field(river_parameters::RiverParameters,river_flow_field::Field{Float64};timestep::Int64=-1)
  variable_name::String = "river_flow"
  filepath::String = timestep == -1 ? "/Users/thomasriddick/Documents/data/temp/river_model_results.nc" : "/Users/thomasriddick/Documents/data/temp/river_model_results_$(timestep).nc"
  write_field(river_parameters.grid,variable_name,river_flow_field,filepath)
end

function load_grid_specific_lake_parameters(file_handle::NcFile,grid::LatLonGrid,
                                            surface_model_grid::LatLonGrid)
  flood_next_cell_lat_index::Field{Int64} =
      load_field(file_handle,grid,"flood_next_cell_lat_index",Int64)
  flood_next_cell_lon_index::Field{Int64} =
      load_field(file_handle,grid,"flood_next_cell_lon_index",Int64)
  connect_next_cell_lat_index::Field{Int64} =
      load_field(file_handle,grid,"connect_next_cell_lat_index",Int64)
  connect_next_cell_lon_index::Field{Int64} =
      load_field(file_handle,grid,"connect_next_cell_lon_index",Int64)
  flood_force_merge_lat_index::Field{Int64} =
      load_field(file_handle,grid,"flood_force_merge_lat_index",Int64)
  flood_force_merge_lon_index::Field{Int64} =
      load_field(file_handle,grid,"flood_force_merge_lon_index",Int64)
  connect_force_merge_lat_index::Field{Int64} =
      load_field(file_handle,grid,"connect_force_merge_lat_index",Int64)
  connect_force_merge_lon_index::Field{Int64} =
      load_field(file_handle,grid,"connect_force_merge_lon_index",Int64)
  flood_redirect_lat_index::Field{Int64} =
      load_field(file_handle,grid,"flood_redirect_lat_index",Int64)
  flood_redirect_lon_index::Field{Int64} =
      load_field(file_handle,grid,"flood_redirect_lon_index",Int64)
  connect_redirect_lat_index::Field{Int64} =
      load_field(file_handle,grid,"connect_redirect_lat_index",Int64)
  connect_redirect_lon_index::Field{Int64} =
      load_field(file_handle,grid,"connect_redirect_lon_index",Int64)
  additional_flood_redirect_lat_index::Field{Int64} =
      load_field(file_handle,grid,"additional_flood_redirect_lat_index",Int64)
  additional_flood_redirect_lon_index::Field{Int64} =
      load_field(file_handle,grid,"additional_flood_redirect_lon_index",Int64)
  additional_connect_redirect_lat_index::Field{Int64} =
      load_field(file_handle,grid,"additional_connect_redirect_lat_index",Int64)
  additional_connect_redirect_lon_index::Field{Int64} =
      load_field(file_handle,grid,"additional_connect_redirect_lon_index",Int64)
  corresponding_surface_cell_lat_index::Field{Int64} =
      load_field(file_handle,surface_model_grid,"corresponding_surface_cell_lat_index",Int64)
  corresponding_surface_cell_lon_index::Field{Int64} =
      load_field(file_handle,surface_model_grid,"corresponding_surface_cell_lon_index",Int64)
  add_offset(flood_next_cell_lat_index,1,Int64[-1])
  add_offset(flood_next_cell_lon_index,1,Int64[-1])
  add_offset(connect_next_cell_lat_index,1,Int64[-1])
  add_offset(connect_next_cell_lon_index,1,Int64[-1])
  add_offset(flood_force_merge_lat_index,1,Int64[-1])
  add_offset(flood_force_merge_lon_index,1,Int64[-1])
  add_offset(connect_force_merge_lat_index,1,Int64[-1])
  add_offset(connect_force_merge_lon_index,1,Int64[-1])
  add_offset(flood_redirect_lat_index,1,Int64[-1])
  add_offset(flood_redirect_lon_index,1,Int64[-1])
  add_offset(connect_redirect_lat_index,1,Int64[-1])
  add_offset(connect_redirect_lon_index,1,Int64[-1])
  add_offset(additional_flood_redirect_lat_index,1,Int64[-1])
  add_offset(additional_flood_redirect_lon_index,1,Int64[-1])
  add_offset(additional_connect_redirect_lat_index,1,Int64[-1])
  add_offset(additional_connect_redirect_lon_index,1,Int64[-1])
  add_offset(corresponding_surface_cell_lat_index,1,Int64[-1])
  add_offset(corresponding_surface_cell_lon_index,1,Int64[-1])
  return LatLonLakeParameters(flood_next_cell_lat_index,
                              flood_next_cell_lon_index,
                              connect_next_cell_lat_index,
                              connect_next_cell_lon_index,
                              flood_force_merge_lat_index,
                              flood_force_merge_lon_index,
                              connect_force_merge_lat_index,
                              connect_force_merge_lon_index,
                              flood_redirect_lat_index,
                              flood_redirect_lon_index,
                              connect_redirect_lat_index,
                              connect_redirect_lon_index,
                              additional_flood_redirect_lat_index,
                              additional_flood_redirect_lon_index,
                              additional_connect_redirect_lat_index,
                              additional_connect_redirect_lon_index,
                              corresponding_surface_cell_lat_index,
                              corresponding_surface_cell_lon_index)
end

function load_grid_specific_lake_parameters(file_handle::NcFile,grid::UnstructuredGrid,
                                            surface_model_grid::UnstructuredGrid)
  flood_next_cell_index::Field{Int64} =
      load_field(file_handle,grid,"flood_next_cell_index",Int64)
  connect_next_cell_index::Field{Int64} =
      load_field(file_handle,grid,"connect_next_cell_index",Int64)
  flood_force_merge_index::Field{Int64} =
      load_field(file_handle,grid,"flood_force_merge_index",Int64)
  connect_force_merge_index::Field{Int64} =
      load_field(file_handle,grid,"connect_force_merge_index",Int64)
  flood_redirect_index::Field{Int64} =
      load_field(file_handle,grid,"flood_redirect_index",Int64)
  connect_redirect_index::Field{Int64} =
      load_field(file_handle,grid,"connect_redirect_index",Int64)
  additional_flood_redirect_index::Field{Int64} =
      load_field(file_handle,grid,"additional_flood_redirect_index",Int64)
  additional_connect_redirect_index::Field{Int64} =
      load_field(file_handle,grid,"additional_connect_redirect_index",Int64)
  corresponding_surface_cell_index::Field{Int64} =
      load_field(file_handle,surface_model_grid,"corresponding_surface_cell_index",Int64)
  return UnstructuredLakeParameters(flood_next_cell_index,
                                    connect_next_cell_index,
                                    flood_force_merge_index,
                                    connect_force_merge_index,
                                    flood_redirect_index,
                                    connect_redirect_index,
                                    additional_flood_redirect_index,
                                    additional_connect_redirect_index,
                                    corresponding_surface_cell_index)
end

function load_lake_initial_values(lake_start_filepath::AbstractString,
                                  grid::Grid,hd_grid::Grid)
  local initial_water_to_lake_centers::Field{Float64}
  local initial_spillover_to_rivers::Field{Float64}
  println("Loading: " * lake_start_filepath)
  file_handle::NcFile = NetCDF.open(lake_start_filepath)
  try
    initial_water_to_lake_centers =
      load_field(file_handle,grid,"water_redistributed_to_lakes",Float64)
    initial_spillover_to_rivers =
      load_field(file_handle,hd_grid,"water_redistributed_to_rivers",Float64)
  finally
    NetCDF.close(file_handle)
  end
  return initial_water_to_lake_centers,initial_spillover_to_rivers
end

function write_lake_numbers_field(lake_parameters::LakeParameters,lake_fields::LakeFields;
                                  timestep::Int64=-1)
  variable_name::String = "lake_field"
  filepath::String = timestep == -1 ? "/Users/thomasriddick/Documents/data/temp/lake_model_results.nc" : "/Users/thomasriddick/Documents/data/temp/lake_model_results_$(timestep).nc"
  write_field(lake_parameters.grid,variable_name,
              lake_fields.lake_numbers,filepath)
end

function write_lake_volumes_field(lake_parameters::LakeParameters,lake_volumes::Field{Float64})
  variable_name::String = "lake_field"
  filepath::String = "/Users/thomasriddick/Documents/data/temp/lake_model_out.nc"
  write_field(lake_parameters.grid,variable_name,lake_volumes,filepath)
end

function write_diagnostic_lake_volumes_field(lake_parameters::LakeParameters,
                                             diagnostic_lake_volumes::Field{Float64};
                                             timestep::Int64=-1)
  variable_name::String = "diagnostic_lake_volumes"
  filepath::String = timestep == -1 ?
                     "/Users/thomasriddick/Documents/data/temp/lake_model_volume_results.nc" :
                     "/Users/thomasriddick/Documents/data/temp/lake_model_volume_results_$(timestep).nc"
  write_field(lake_parameters.grid,variable_name,diagnostic_lake_volumes,filepath)
end

function load_drainage_fields(drainages_filename::AbstractString,grid::Grid;
                              first_timestep::Int64=1,last_timestep::Int64=1)
  println("Loading: " * drainages_filename)
  file_handle::NcFile = NetCDF.open(drainages_filename)
  drainages::Array{Field{Float64},1} = Field{Float64}[]
  try
    for i::Int64 = first_timestep:last_timestep
      drainages[i] =  load_field(file_handle,grid,"drainages",
                                 Float64;timestep=i)
    end
  finally
    NetCDF.close(file_handle)
  end
  return drainages
end

function load_runoff_fields(runoffs_filename::AbstractString,grid::Grid;
                            first_timestep::Int64=1,last_timestep::Int64=1)
  println("Loading: " * runoffs_filename)
  file_handle::NcFile = NetCDF.open(runoffs_filename)
  runoffs::Array{Field{Float64},1} = Field{Float64}[]
  try
    for i::Int64 = first_timestep:last_timestep
      runoffs[i] = load_field(file_handle,grid,"runoffs",
                              Float64;timestep=i)
    end
  finally
    NetCDF.close(file_handle)
  end
  return runoffs
end

function load_lake_evaporation_fields(lake_evaporations_filename::AbstractString,
                                      grid::LatLonGrid;first_timestep::Int64=1,
                                      last_timestep::Int64=1)
  println("Loading: " * lake_evaporations_filename)
  file_handle::NcFile = NetCDF.open(lake_evaporations_filename)
  lake_evaporations::Array{Field{Float64},1} = Field{Float64}[]
  try
    for i::Int64 = first_timestep:last_timestep
      lake_evaporations[i] = load_field(file_handle,grid,"evaporation",
                              Float64;timestep=i)
    end
  finally
    NetCDF.close(file_handle)
  end
  return lake_evaporations
end

end


struct EconomicDispatchProblem <: PowerSimulationsOperationsProblem end
struct UnitCommitmentProblem <: PowerSimulationsOperationsProblem end
struct AGCReserveDeployment <: PowerSimulationsOperationsProblem end

function _default_devices_uc()
    return [
        DeviceModel(PSY.ThermalStandard, ThermalBasicUnitCommitment),
        DeviceModel(PSY.RenewableDispatch, RenewableFullDispatch),
        DeviceModel(PSY.RenewableFix, FixedOutput),
        DeviceModel(PSY.HydroEnergyReservoir, HydroDispatchRunOfRiver),
        DeviceModel(PSY.HydroDispatch, HydroDispatchRunOfRiver),
        DeviceModel(PSY.PowerLoad, StaticPowerLoad),
        DeviceModel(PSY.InterruptibleLoad, InterruptiblePowerLoad),
        DeviceModel(PSY.Line, StaticBranch),
        DeviceModel(PSY.Transformer2W, StaticBranch),
        DeviceModel(PSY.TapTransformer, StaticBranch),
        DeviceModel(PSY.HVDCLine, HVDCDispatch),
    ]
end

function _default_devices_dispatch()
    default = _default_devices_uc()
    default[1] = DeviceModel(PSY.ThermalStandard, ThermalDispatch)
    return default
end

function _default_services()
    return [
        ServiceModel(PSY.VariableReserve{PSY.ReserveUp}, RangeReserve),
        ServiceModel(PSY.VariableReserve{PSY.ReserveDown}, RangeReserve),
    ]
end

"""
    template_unit_commitment(; kwargs...)

Creates an `OperationsProblemTemplate` with default DeviceModels for a Unit Commitment
problem.

# Example
```julia
template = template_unit_commitment()
```

# Accepted Key Words
- `network::Type{<:PM.AbstractPowerModel}` : override default network model settings
- `devices::Vector{DeviceModel}` : override default `DeviceModel` settings
- `services::Vector{ServiceModel}` : override default `ServiceModel` settings
"""
function template_unit_commitment(; kwargs...)
    network = get(kwargs, :network, CopperPlatePowerModel)
    template = OperationsProblemTemplate(network)
    for model in get(kwargs, :devices, _default_devices_uc())
        set_device_model!(template, model)
    end

    for model in get(kwargs, :services, _default_services())
        set_service_model!(template, model)
    end
    return template
end

"""
    template_economic_dispatch(; kwargs...)

Creates an `OperationsProblemTemplate` with default DeviceModels for an Economic Dispatch
problem.

# Example
```julia
template = template_economic_dispatch()
```

# Accepted Key Words
- `network::Type{<:PM.AbstractPowerModel}` : override default network model settings
- `devices::Vector{DeviceModel}` : override default `DeviceModel` settings
- `services::Vector{ServiceModel}` : override default `ServiceModel` settings
"""
function template_economic_dispatch(; kwargs...)
    network = get(kwargs, :network, CopperPlatePowerModel)
    template = OperationsProblemTemplate(network)
    for model in get(kwargs, :devices, _default_devices_dispatch())
        set_device_model!(template, model)
    end

    for model in get(kwargs, :services, _default_services())
        set_service_model!(template, model)
    end

    return template
end

"""
    template_agc_reserve_deployment(; kwargs...)

Creates an `OperationsProblemTemplate` with default DeviceModels for an AGC Reserve Deplyment Problem. This model doesn't support customization

# Example
```julia
template = agc_reserve_deployment()
```
"""
function template_agc_reserve_deployment(; kwargs...)
    if !isempty(kwargs)
        throw(ArgumentError("AGC Template doesn't currently support customization"))
    end
    template = OperationsProblemTemplate(AreaBalancePowerModel)
    set_device_model!(template, PSY.ThermalStandard, FixedOutput)
    set_device_model!(template, PSY.RenewableDispatch, FixedOutput)
    set_device_model!(template, PSY.PowerLoad, StaticPowerLoad)
    set_device_model!(template, PSY.HydroEnergyReservoir, FixedOutput)
    set_device_model!(template, PSY.HydroDispatch, FixedOutput)
    set_device_model!(template, PSY.RenewableFix, FixedOutput)
    set_device_model!(
        template,
        DeviceModel(PSY.RegulationDevice{PSY.ThermalStandard}, DeviceLimitedRegulation),
    )
    set_device_model!(
        template,
        DeviceModel(PSY.RegulationDevice{PSY.HydroDispatch}, ReserveLimitedRegulation),
    )
    set_device_model!(
        template,
        DeviceModel(
            PSY.RegulationDevice{PSY.HydroEnergyReservoir},
            ReserveLimitedRegulation,
        ),
    )
    set_service_model!(template, ServiceModel(PSY.AGC, PIDSmoothACE))
    return template
end


export ProblemDefinition, Trajectory
export TrajectoryOptimizationProblem, TrajectoryOptimizationSolution

abstract type Robot end
abstract type DynamicsModel end
abstract type Environment end

mutable struct Workspace
	btenvironment_keepin
	btenvironment_keepout
end
function Workspace(rb::Robot, env::Environment)
  btenv_keepin = BulletCollision.BulletStaticEnvironment(rb.btCollisionObject, BulletCollision.collision_world(env.worldAABBmin, env.worldAABBmax))
  for zone in env.keepin_zones
    BulletCollision.add_collision_object!(btenv_keepin, BulletCollision.geometry_type_to_BT(zone))
  end

  btenv_keepout = BulletCollision.BulletStaticEnvironment(rb.btCollisionObject, BulletCollision.collision_world(env.worldAABBmin, env.worldAABBmax))
  for zone in (env.keepout_zones..., env.obstacle_set...)
    BulletCollision.add_collision_object!(btenv_keepout, BulletCollision.geometry_type_to_BT(zone))
  end

  Workspace(btenv_keepin, btenv_keepout)
end

abstract type GoalType end
mutable struct GoalSet
	goals::SortedMultiDict
end
GoalSet() = GoalSet(SortedMultiDict())

mutable struct ProblemDefinition{R<:Robot, D<:DynamicsModel, E<:Environment}
	robot::R
	model::D
	env::E

	x_init
	goal_set
end

mutable struct Trajectory
	X 	# x_dim x N
	U
	Tf
	dt
end

mutable struct TrajectoryOptimizationProblem{R<:Robot, D<:DynamicsModel, E<:Environment}
	PD::ProblemDefinition{R,D,E}
  WS::Workspace
	fixed_final_time::Bool

	N 				# Discretization steps
	tf_guess	# Guess for final time
	dh				# Normalized dt
end
function TrajectoryOptimizationProblem(PD, fixed_final_time, N, tf_guess, dh)
  WS = Workspace(PD.robot, PD.env)
  assign_timesteps!(PD.goal_set, N, tf_guess)
  TrajectoryOptimizationProblem(PD,WS,fixed_final_time,N,tf_guess,dh)
end

abstract type SCPParamSpecial end

mutable struct SCPParam
	fixed_final_time::Bool
	convergence_threshold
	obstacle_toggle_distance

	alg::SCPParamSpecial	# Algorithm-specific parameters

	SCPParam(a::Bool, b) = new(a,b)
end

# Optimization algorithm problem
abstract type OptAlgorithmProblem{R<:Robot, D<:DynamicsModel, E<:Environment} end

mutable struct SCPProblem{R,D,E} <: OptAlgorithmProblem{R,D,E}
	PD::ProblemDefinition{R,D,E}
  WS::Workspace
	param::SCPParam

	N 				# Discretization steps
	tf_guess	# Guess for final time
	dh				# Normalized dt
end

# TODO: Remove Convex.jl
VariableTypes = Union{Convex.Variable, JuMP.VariableRef}

mutable struct SCPVariables{T <: VariableTypes, S<:Union{T,Array{T}}}
	X::S 		# State trajectory
	U::S 		# Control trajectory
	Tf::T 	# Final time

	SCPVariables{T,S}() where {T <: VariableTypes, S<:Union{T,Array{T}}} = new()
	SCPVariables{T,S}(X,U) where {T <: VariableTypes, S<:Union{T,Array{T}}} = new(X,U)
	SCPVariables{T,S}(X,U,Tf) where {T <: VariableTypes, S<:Union{T,Array{T}}} = new(X,U,Tf)
end
SCPVariables(X::S,U::S) where {S<:Union{VariableTypes,Array{VariableTypes}}} = SCPVariables{T,S}(X,U)
SCPVariables(X::S,U::S,Tf::T) where {T <: VariableTypes, S<:Union{T,Array{T}}} = SCPVariables{T,S}(X,U,Tf)
function SCPVariables{T,S}(SCPP::SCPProblem) where {T <: Convex.Variable, S<:Union{T,Array{T}}}
	X = Convex.Variable(SCPP.PD.model.x_dim, SCPP.N)
	U = Convex.Variable(SCPP.PD.model.u_dim, SCPP.N)
	Tf = Convex.Variable(1)
	SCPP.param.fixed_final_time ? fix!(Tf, SCPP.tf_guess) : nothing
	SCPVariables{T,S}(X,U,Tf)
end

SolverStatusTypes = Union{Symbol, MOI.TerminationStatusCode}

mutable struct SCPConstraints
	## Dynamics constraints
	dynamics::Dict 												# Should be linearized

	## General state constraints
	convex_state_eq::Dict 								# Should be linearized
	nonconvex_state_eq::Dict
	nonconvex_state_convexified_eq::Dict 	# Should be linearized

	convex_state_ineq::Dict
	nonconvex_state_ineq::Dict
	nonconvex_state_convexified_ineq::Dict

	## Boundary condition state constraints
	state_init_eq::Dict
	convex_state_boundary_condition_eq::Dict
	nonconvex_state_boundary_condition_eq::Dict
	nonconvex_state_boundary_condition_convexified_eq::Dict

	# These should be approximations of equality constraints,
	# and treated as equality constraints in the shooting method
	convex_state_boundary_condition_ineq::Dict
	nonconvex_state_boundary_condition_ineq::Dict
	nonconvex_state_boundary_condition_convexified_ineq::Dict

	## Control constraints
	convex_control_eq::Dict
	convex_control_ineq::Dict

	## Trust region constraints
	state_trust_region_ineq::Dict
	control_trust_region_ineq::Dict

	# TODO(acauligi): only convex_state_bc_ineq, nonconvex_state_bc_ineq, nonconvex_state_bc_convexified_ineq
	#   are correctly implemented across algorithms
end
SCPConstraints() = SCPConstraints((Dict{Symbol, Vector}() for i in 1:18)...)

mutable struct SCPSolution
	traj::Trajectory
	dual::Vector

	J_true::Vector
	J_full::Vector
	solver_status::Vector{SolverStatusTypes}	# Solver status
	scp_status::Vector											 	# SCP algorithm status
	accept_solution::Vector 									# Solution accepted?

	convergence_measure::Vector								# Convergence measure of the latest iteration
	successful::Bool 													# Did we find an acceptable solution?
	converged::Bool														# Has the solution met the convergence condition?
	iterations::Int 													# Number of SCP iterations executed
	iter_elapsed_times::Vector
	total_time 	# TODO(ambyld): Move to TOP

	param::SCPParam
	SCPP::SCPProblem
	SCPC::SCPConstraints
	solver_model

	SCPSolution(a,b,c,d,e,f,g,h,i,j,k,l,m,n,o) = new(a,b,c,d,e,f,g,h,i,j,k,l,m,n,o)
end

mutable struct ConstraintCategory
	func
	dimtype
	ind_time
	ind_other
	params
	con_reference
	var_reference
	ConstraintCategory(a,b,c,d) = new(a,b,c,d)
	ConstraintCategory(a,b,c,d,e) = new(a,b,c,d,e)
end

mutable struct ShootingProblem{R,D,E} <: OptAlgorithmProblem{R,D,E}
	PD::ProblemDefinition{R,D,E}
	WS::Workspace

	p0 	# Initial dual
	N 	# Discretization steps
	tf  # Final time
	dt
	x_goal
end

mutable struct ShootingSolution
	traj::Trajectory

	J_true::Vector
	prob_status::Vector
	convergence_measure::Vector
	converged::Bool
	iter_elapsed_times::Vector

	SP::ShootingProblem
end
ShootingSolution(SP, traj_init) = ShootingSolution(traj_init, [], [:(NA)], [NaN], false, [0.], SP)

mutable struct TrajectoryOptimizationSolution
	traj::Trajectory
	SCPS::SCPSolution
	SS::ShootingSolution
	total_time
	TrajectoryOptimizationSolution(TOP::TrajectoryOptimizationProblem) = new(Trajectory(TOP))
end

function ShootingProblem(TOP::TrajectoryOptimizationProblem, SCPS::SCPSolution)
	goal_set, tf_guess, x_dim = TOP.PD.goal_set, TOP.tf_guess, TOP.PD.model.x_dim
	x_goal = zeros(x_dim)
  for goal in values(inclusive(goal_set.goals, searchsortedfirst(goal_set.goals, tf_guess), searchsortedlast(goal_set.goals, tf_guess)))
    x_goal[goal.ind_coordinates] = center(goal.params)
  end
  	N = TOP.N
	ShootingProblem(TOP.PD, TOP.WS, SCPS.dual, TOP.N, SCPS.traj.Tf, SCPS.traj.Tf/(N-1), x_goal)
end

TrajectoryOptimizationProblem(PD, N, tf_guess; fixed_final_time::Bool=false) = TrajectoryOptimizationProblem(PD, fixed_final_time, N, tf_guess, 1/(N-1))

# Initialize a blank trajectory optimization solution
# TrajectoryOptimizationSolution(TOP::TrajectoryOptimizationProblem) = TrajectoryOptimizationSolution(Trajectory(TOP))

SCPSolution(SCPP::SCPProblem, traj_init::Trajectory) = SCPSolution(traj_init, [], [], [], [:(NA)], [:(NA)], [true], [0.], false, false, 0, [0.], 0., SCPP.param, SCPP)

Trajectory(X, U, Tf) = Trajectory(X, U, Tf, Tf/(size(X,2)-1))

# Initialize a blank trajectory structure
function Trajectory(TOP::TrajectoryOptimizationProblem)
	x_dim = TOP.PD.model.x_dim
	u_dim = TOP.PD.model.u_dim
	N, tf_guess = TOP.N, TOP.tf_guess
	dt = tf_guess/(N-1)

	Trajectory(zeros(x_dim,N), zeros(u_dim,N), tf_guess, dt)
end

function Base.copy!(a::Trajectory, b::Trajectory)
	a.X = deepcopy(b.X)
	a.U = deepcopy(b.U)
	a.Tf = deepcopy(b.Tf)
	a.dt = deepcopy(b.dt)
end

Base.deepcopy(a::Trajectory) = Trajectory(deepcopy(a.X), deepcopy(a.U), deepcopy(a.Tf), deepcopy(a.dt))

function SCPProblem(TOP::TrajectoryOptimizationProblem)
	N = TOP.N
	SCPProblem(TOP.PD, TOP.WS, SCPParam(TOP.PD.model, TOP.fixed_final_time), N, TOP.tf_guess, 1/(N-1))
end

function SCPParam(model::DynamicsModel, fixed_final_time::Bool)
  convergence_threshold = 0.001
  obstacle_toggle_distance = Inf

  SCPParam(fixed_final_time, convergence_threshold, obstacle_toggle_distance)
end