struct ConvergenceState
   obj::Float64
   ub::Float64
   lb::Float64
   int_abs::Float64
   int_rel::Float64
   rlx_abs::Float64
   rlx_rel::Float64
   time::Float64
   iter::Int
   num_frac::Int
   function ConvergenceState(obj::Float64, ub::Float64, lb::Float64, int_abs::Float64, int_rel::Float64,
                             rlx_abs::Float64, rlx_rel::Float64, time::Float64, iter::Int, num_frac::Int)
      return new(obj, ub, lb, int_abs, int_rel, rlx_abs, rlx_rel, time, iter, num_frac)
   end
   function ConvergenceState(obj::Float64, ub::Float64, lb::Float64, time::Float64, iter::Int, num_frac::Int)
      return new(obj, ub, lb, ub-lb, (ub-lb)/abs(lb), obj-lb, (obj-lb)/abs(lb), time, iter, num_frac)
   end
end

mutable struct Termination
   abstol::Float64
   reltol::Float64
   rlx_abstol::Float64
   rlx_reltol::Float64
   time_limit::Float64
   max_iter::Int
   inttol::Float64
   allow_frac::Symbol

   """
   Termination(;abstol::Float64=10^-10,
               reltol::Float64=10^-10,
               rlx_abstol::Float64=10^-10,
               rlx_reltol::Float64=10^-10,
               time_limit::Float64=Inf,
               max_iter::Int=2147483647,
               inttol::Float64=10^-9,
               allow_frac::Symbol=:binary_solve)

   Define the stopping conditions for `JuDGE.solve()` / `JuDGE.branch_and_price()`.

   ### Optional Arguments
   `abstol` is the absolute tolerance for the best integer-feasible objective value and the lower bound.

   `reltol` is the relative tolerance for the best integer-feasible objective value and the lower bound.

   `rlx_abstol` is the absolute tolerance for the relaxed master objective value and the lower bound.

   `rlx_reltol` is the relative tolerance for the relaxed master objective value and the lower bound.

   `time_limit` is the maximum duration in seconds.

   `max_iter` is the maximum number of iterations.

   `inttol` is the maximum deviation from 0 or 1 for any binary/integer variable for integer feasible solutions.

   `allow_frac` indicates whether a fractional solution will be returned; possible values are:
   	`:binary_solve` a binary solve of master will be performed (if needed) prior to the solution being returned;
   	`:binary_solve_return_relaxation` a binary solve of master will be performed (if needed), updating the upper bound,
   	but the master problem relation will be returned;
   	`:first_fractional` will return the first fractional master solution found;
   	`:no_binary_solve` will simply return the solution to the relaxed master problem when terminated.

   ### Examples
      Termination(rlx_abstol=10^-6)
      Termination(abstol=10^-3,inttol=10^-6)
   """
   function Termination(;abstol::Float64=10^-10,reltol::Float64=10^-10,rlx_abstol::Float64=10^-10,
                        rlx_reltol::Float64=10^-10,time_limit::Float64=Inf,max_iter::Int=2147483647,
                        inttol::Float64=10^-9,allow_frac::Symbol=:binary_solve)
      if allow_frac ∉ [:binary_solve,:binary_solve_return_relaxation,:first_fractional,:no_binary_solve]
         error("'allow_frac' can take values: :binary_solve, :binary_solve_return_relaxation, :first_fractional or :no_binary_solve")
      end
      return new(abstol, reltol, rlx_abstol, rlx_reltol, time_limit, max_iter, inttol, allow_frac)
   end
end

function display(cs::ConvergenceState;relaxation::Bool=true)
   print("")
   if cs.obj==Inf
      print("          ")
   elseif cs.obj>=0
      print(" ")
   end
   Printf.@printf(" %e  | ",cs.obj)

   if cs.ub==Inf
      print("          ")
   elseif cs.ub>=0
      print(" ")
   end
   Printf.@printf("%e ",cs.ub)

   if cs.lb==-Inf
      print("         ")
   elseif cs.lb>=0
      print(" ")
   end
   Printf.@printf("%e  |  ",cs.lb)

   if relaxation
      temp1=cs.rlx_abs
      temp2=cs.rlx_rel
   else
      temp1=cs.int_abs
      temp2=cs.int_rel
   end

   if temp1==Inf
      print("          ")
   elseif temp1>=0
      print(" ")
   end
   Printf.@printf("%e   ",temp1)

   if temp2==Inf || isnan(temp2)
      print("          ")
   elseif temp2>=0
      print(" ")
   end
   Printf.@printf("%e  |   ",temp2)

   Printf.@printf("%9d  | %9.3f  %7d",cs.num_frac,cs.time,cs.iter)
   if relaxation
      println("")
   else
      println("*")
   end
end

function InitialConvergenceState()
   ConvergenceState(Inf,Inf,-Inf,Inf,Inf,Inf,Inf,0.0,0,0)
end

function has_converged(b::Termination, a::ConvergenceState)
   if a.int_abs <= b.abstol || a.int_rel <= b.reltol || a.rlx_abs <= b.rlx_abstol || a.rlx_rel <= b.rlx_reltol || a.time > b.time_limit || a.iter >= b.max_iter
      true
   else
      false
   end
end


value(x) = x
cuify(x) = error("To use LinSolveGPUFactorize, you must do `using CuArrays`")

# Piracy, should get upstreamed
LinearAlgebra.ldiv!(Y::AbstractArray, A::AbstractArray, B::AbstractArray) = (copyto!(Y,B); ldiv!(A,Y))

function __init__()
  @require ApproxFun="28f2ccd6-bb30-5033-b560-165f7b14dc2f" begin
    eval_u0(u0::ApproxFun.Fun) = false
  end

  @require Distributions="31c24e10-a181-5473-b8eb-7969acd0382f" begin
    handle_distribution_u0(_u0::Distributions.Sampleable) = rand(_u0)
  end

  @require ForwardDiff="f6369f11-7733-5829-9624-2563aa707210" begin

    value(x::Type{ForwardDiff.Dual{T,V,N}}) where {T,V,N} = V
    value(x::ForwardDiff.Dual) = value(ForwardDiff.value(x))

    @inline fastpow(x::ForwardDiff.Dual, y::ForwardDiff.Dual) = x^y

    # Support adaptive with non-dual time
    @inline function ODE_DEFAULT_NORM(u::AbstractArray{<:ForwardDiff.Dual,N},t) where {N}
      sqrt(sum(x->ODE_DEFAULT_NORM(x[1],x[2]),zip((value(x) for x in u),Iterators.repeated(t))) / length(u))
    end
    @inline function ODE_DEFAULT_NORM(u::Array{<:ForwardDiff.Dual,N},t) where {N}
      sqrt(sum(x->ODE_DEFAULT_NORM(x[1],x[2]),zip((value(x) for x in u),Iterators.repeated(t))) / length(u))
    end
    @inline ODE_DEFAULT_NORM(u::ForwardDiff.Dual,t) = abs(value(u))

    # When time is dual, it shouldn't drop the duals for adaptivity
    @inline function ODE_DEFAULT_NORM(u::AbstractArray{<:ForwardDiff.Dual,N},t::ForwardDiff.Dual) where {N}
      sqrt(sum(x->ODE_DEFAULT_NORM(x[1],x[2]),zip((x for x in u),Iterators.repeated(t))) / length(u))
    end
    @inline function ODE_DEFAULT_NORM(u::Array{<:ForwardDiff.Dual,N},t::ForwardDiff.Dual) where {N}
      sqrt(sum(x->ODE_DEFAULT_NORM(x[1],x[2]),zip((x for x in u),Iterators.repeated(t))) / length(u))
    end
    @inline ODE_DEFAULT_NORM(u::ForwardDiff.Dual,t::ForwardDiff.Dual) = abs(u)

    # Type piracy. Should upstream
    Base.nextfloat(d::ForwardDiff.Dual{T,V,N}) where {T,V,N} = ForwardDiff.Dual{T}(nextfloat(d.value), d.partials)
    Base.prevfloat(d::ForwardDiff.Dual{T,V,N}) where {T,V,N} = ForwardDiff.Dual{T}(prevfloat(d.value), d.partials)

    struct DiffCache{T<:AbstractArray, S<:AbstractArray}
        du::T
        dual_du::S
    end

    function DiffCache(u::AbstractArray{T}, siz, ::Type{Val{chunk_size}}) where {T, chunk_size}
        DiffCache(u, zeros(ForwardDiff.Dual{nothing,T,chunk_size}, siz...))
    end

    dualcache(u::AbstractArray, N=Val{ForwardDiff.pickchunksize(length(u))}) = DiffCache(u, size(u), N)

    get_tmp(dc::DiffCache, u::AbstractArray{T}) where T<:ForwardDiff.Dual = reinterpret(T, dc.dual_du)
    get_tmp(dc::DiffCache, u::AbstractArray) = dc.du
  end

  @require Measurements="eff96d63-e80a-5855-80a2-b1b0885c5ab7" begin

    value(x::Type{Measurements.Measurement{T}}) where {T} = T
    value(x::Measurements.Measurement) = Measurements.value(x)

    # Support adaptive steps should be errorless
    @inline function ODE_DEFAULT_NORM(u::AbstractArray{<:Measurements.Measurement,N},t) where {N}
      sqrt(sum(x->ODE_DEFAULT_NORM(x[1],x[2]),zip((value(x) for x in u),Iterators.repeated(t))) / length(u))
    end
    @inline function ODE_DEFAULT_NORM(u::Array{<:Measurements.Measurement,N},t) where {N}
      sqrt(sum(x->ODE_DEFAULT_NORM(x[1],x[2]),zip((value(x) for x in u),Iterators.repeated(t))) / length(u))
    end
    @inline ODE_DEFAULT_NORM(u::Measurements.Measurement,t) = abs(Measurements.value(u))
  end

  @require MonteCarloMeasurements="0987c9cc-fe09-11e8-30f0-b96dd679fdca" begin

    value(x::Type{MonteCarloMeasurements.AbstractParticles{T,N}}) where {T,N} = T
    value(x::MonteCarloMeasurements.AbstractParticles) = mean(x)

    # Support adaptive steps should be errorless
    @inline function ODE_DEFAULT_NORM(u::AbstractArray{<:MonteCarloMeasurements.AbstractParticles,N},t) where {N}
      sqrt(mean(x->ODE_DEFAULT_NORM(x[1],x[2]),zip((value(x) for x in u),Iterators.repeated(t))))
    end
    @inline function ODE_DEFAULT_NORM(u::AbstractArray{<:MonteCarloMeasurements.AbstractParticles,N},t::AbstractArray{<:MonteCarloMeasurements.AbstractParticles,N}) where {N}
      sqrt(mean(x->ODE_DEFAULT_NORM(x[1],x[2]),zip((value(x) for x in u),Iterators.repeated(value.(t)))))
    end
    @inline ODE_DEFAULT_NORM(u::MonteCarloMeasurements.AbstractParticles,t) = abs(value(u))
  end

  @require Unitful="1986cc42-f94f-5a68-af5c-568840ba703d" begin
    # Support adaptive errors should be errorless for exponentiation
    value(x::Type{Unitful.AbstractQuantity{T,D,U}}) where {T,D,U} = T
    value(x::Unitful.AbstractQuantity) = x.val
    @inline function ODE_DEFAULT_NORM(u::AbstractArray{<:Unitful.AbstractQuantity,N},t) where {N}
      sqrt(sum(x->ODE_DEFAULT_NORM(x[1],x[2]),zip((value(x) for x in u),Iterators.repeated(t))) / length(u))
    end
    @inline function ODE_DEFAULT_NORM(u::Array{<:Unitful.AbstractQuantity,N},t) where {N}
      sqrt(sum(x->ODE_DEFAULT_NORM(x[1],x[2]),zip((value(x) for x in u),Iterators.repeated(t))) / length(u))
    end
    @inline ODE_DEFAULT_NORM(u::Unitful.AbstractQuantity,t) = abs(value(u))
    @inline UNITLESS_ABS2(x::Unitful.AbstractQuantity) = real(abs2(x)/oneunit(x)*oneunit(x))
  end

  @require Tracker="9f7883ad-71c0-57eb-9f7f-b5c9e6d3789c" begin
    value(x::Type{Tracker.TrackedReal{T}}) where T = T
    value(x::Type{Tracker.TrackedArray{T,N,A}}) where {T,N,A} = Array{T,N}
    value(x::Tracker.TrackedReal)  = x.data
    value(x::Tracker.TrackedArray) = x.data

    # Support adaptive with non-tracked time
    @inline function ODE_DEFAULT_NORM(u::Tracker.TrackedArray,t) where {N}
      sqrt(sum(x->ODE_DEFAULT_NORM(x[1],x[2]),zip((value(x) for x in u),Iterators.repeated(t))) / length(u))
    end
    @inline function ODE_DEFAULT_NORM(u::AbstractArray{<:Tracker.TrackedReal,N},t) where {N}
      sqrt(sum(x->ODE_DEFAULT_NORM(x[1],x[2]),zip((value(x) for x in u),Iterators.repeated(t))) / length(u))
    end
    @inline function ODE_DEFAULT_NORM(u::Array{<:Tracker.TrackedReal,N},t) where {N}
      sqrt(sum(x->ODE_DEFAULT_NORM(x[1],x[2]),zip((value(x) for x in u),Iterators.repeated(t))) / length(u))
    end
    @inline ODE_DEFAULT_NORM(u::Tracker.TrackedReal,t) = abs(value(u))

    # Support TrackedReal time, don't drop tracking on the adaptivity there
    @inline function ODE_DEFAULT_NORM(u::Tracker.TrackedArray,t::Tracker.TrackedReal) where {N}
      sqrt(sum(x->ODE_DEFAULT_NORM(x[1],x[2]),zip(u,Iterators.repeated(t))) / length(u))
    end
    @inline function ODE_DEFAULT_NORM(u::AbstractArray{<:Tracker.TrackedReal,N},t::Tracker.TrackedReal) where {N}
      sqrt(sum(x->ODE_DEFAULT_NORM(x[1],x[2]),zip(u,Iterators.repeated(t))) / length(u))
    end
    @inline function ODE_DEFAULT_NORM(u::Array{<:Tracker.TrackedReal,N},t::Tracker.TrackedReal) where {N}
      sqrt(sum(x->ODE_DEFAULT_NORM(x[1],x[2]),zip(u,Iterators.repeated(t))) / length(u))
    end
    @inline ODE_DEFAULT_NORM(u::Tracker.TrackedReal,t::Tracker.TrackedReal) = abs(u)
  end

  # Piracy, should get upstreamed
  @require CuArrays="3a865a2d-5b23-5a0f-bc46-62713ec82fae" begin
    cuify(x::AbstractArray) = CuArrays.CuArray(x)
    function LinearAlgebra.ldiv!(x::CuArrays.CuArray,_qr::CuArrays.CUSOLVER.CuQR,b::CuArrays.CuArray)
      _x = UpperTriangular(_qr.R) \ (_qr.Q' * reshape(b,length(b),1))
      x .= vec(_x)
      CuArrays.unsafe_free!(_x)
    end
    function findall_events(affect!,affect_neg!,prev_sign::CuArrays.CuArray,next_sign::CuArrays.CuArray)
      hasaffect::Bool = affect! !== nothing
      hasaffectneg::Bool = affect_neg! !== nothing
      f = (p,n)-> ((p < 0 && hasaffect) || (p > 0 && hasaffectneg)) && p*n<=0
      A = map(f,prev_sign,next_sign)
      out = findall(A)
      CuArrays.unsafe_free!(A)
      out
    end
  end
end


"""
For observed data without missings.

# Constructor

    SemObservedData(;
        specification,
        data,
        meanstructure = false,
        obs_colnames = nothing,
        kwargs...)

# Arguments
- `specification`: either a `RAMMatrices` or `ParameterTable` object (1)
- `data`: observed data
- `meanstructure::Bool`: does the model have a meanstructure?
- `obs_colnames::Vector{Symbol}`: column names of the data (if the object passed as data does not have column names, i.e. is not a data frame)

# Extended help
## Interfaces
- `n_obs(::SemObservedData)` -> number of observed data points
- `n_man(::SemObservedData)` -> number of manifest variables

- `get_data(::SemObservedData)` -> observed data
- `obs_cov(::SemObservedData)` -> observed.obs_cov
- `obs_mean(::SemObservedData)` -> observed.obs_mean
- `data_rowwise(::SemObservedData)` -> observed data, stored as vectors per observation

## Implementation
Subtype of `SemObserved`

## Remarks
(1) the `specification` argument can also be `nothing`, but this turns of checking whether
the observed data/covariance columns are in the correct order! As a result, you should only
use this if you are shure your observed data is in the right format.

## Additional keyword arguments:
- `spec_colnames::Vector{Symbol} = nothing`: overwrites column names of the specification object
- `compute_covariance::Bool ) = true`: should the covariance of `data` be computed and stored?
- `rowwise::Bool = false`: should the data be stored also as vectors per observation
"""
struct SemObservedData{A, B, C, D, O, R} <: SemObserved
    data::A
    obs_cov::B
    obs_mean::C
    n_man::D
    n_obs::O
    data_rowwise::R
end

# error checks
function check_arguments_SemObservedData(kwargs...)
    # data is a data frame,

end

function SemObservedData(;
        specification,
        data,

        obs_colnames = nothing,
        spec_colnames = nothing,

        meanstructure = false,
        compute_covariance = true,

        rowwise = false,

        kwargs...)

    if isnothing(spec_colnames) spec_colnames = get_colnames(specification) end

    if !isnothing(spec_colnames)
        if isnothing(obs_colnames)
            try
                data = data[:, spec_colnames]
            catch
                throw(ArgumentError(
                    "Your `data` can not be indexed by symbols. "*
                    "Maybe you forgot to provide column names via the `obs_colnames = ...` argument.")
                    )
            end
        else
            if data isa DataFrame
                throw(ArgumentError(
                    "You passed your data as a `DataFrame`, but also specified `obs_colnames`. "*
                    "Please make shure the column names of your data frame indicate the correct variables "*
                    "or pass your data in a different format.")
                    )
            end

            if !(eltype(obs_colnames) <: Symbol)
                throw(ArgumentError("please specify `obs_colnames` as a vector of Symbols"))
            end

            data = reorder_data(data, spec_colnames, obs_colnames)
        end
    end

    if data isa DataFrame
        data = Matrix(data)
    end

    n_obs, n_man = Float64.(size(data))

    if compute_covariance
        obs_cov = Statistics.cov(data)
    else
        obs_cov = nothing
    end

    # if a meanstructure is needed, compute observed means
    if meanstructure
        obs_mean = vcat(Statistics.mean(data, dims = 1)...)
    else
        obs_mean = nothing
    end

    if rowwise
        data_rowwise = [data[i, :] for i = 1:convert(Int64, n_obs)]
    else
        data_rowwise = nothing
    end

    return SemObservedData(data, obs_cov, obs_mean, n_man, n_obs, data_rowwise)
end

############################################################################################
### Recommended methods
############################################################################################

n_obs(observed::SemObservedData) = observed.n_obs
n_man(observed::SemObservedData) = observed.n_man

############################################################################################
### additional methods
############################################################################################

get_data(observed::SemObservedData) = observed.data
obs_cov(observed::SemObservedData) = observed.obs_cov
obs_mean(observed::SemObservedData) = observed.obs_mean
data_rowwise(observed::SemObservedData) = observed.data_rowwise

############################################################################################
### Additional functions
############################################################################################

# reorder data -----------------------------------------------------------------------------
function reorder_data(data::AbstractArray, spec_colnames, obs_colnames)
    if spec_colnames == obs_colnames
        return data
    else
        new_position = [findall(x .== obs_colnames)[1] for x in spec_colnames]
        data = data[:, new_position]
        return data
    end
end


# Start here to test the policy-compliance model
using Pkg
Pkg.activate(".")

# load other packages
using Agents

# Load submodules of the simulations
include(joinpath("..", "src", "PolicyCompliance.jl"))

include("../src/simulation/simulation.jl")

# run the actual simulation

abm = initialize_model()
(data, mdata) = runsimulation(abm)

#data_export tbd


module Combinations

using ACE, JuLIP, ForwardDiff, LinearAlgebra
using JuLIP.MLIPs: IPBasis
using ACE.RPI: RPIBasis

import JuLIP.Potentials: zlist, z2i, i2z, numz,
                         evaluate!, evaluate_d!,
                         alloc_temp, alloc_temp_d,
                         evaluate, evaluate_d,
                         cutoff

import JuLIP: fltype

export FitCombiPotential,
       num_features

@doc raw"""
`mutable struct FitCombiPotential`: Defines a site potential of the form
```math
\begin{aligned}
   E_i &= F(\rho_1, \dots, \rho_P), \\
   \rho_p &= \sum_k \theta_{pk} B_k
\end{aligned}
where $$B_k$$ are the basis functions and $$\theta_{pk}$$ the parameters.
```

### Fields
* `basis` : any IPBasis is allowed for now
* `params` : the parameters, a matrix; cf above.
* `F` : the nonlinear outer function.

It is assumed that the output of F is also `::T`
"""
mutable struct FitCombiPotential{T, TP, TF, NZ} <: SitePotential
   basis::RPIBasis{T}
   params::NTuple{NZ, Matrix{TP}}
   F::TF
end

zlist(V::FitCombiPotential) = zlist(V.basis)
# z2i(V::FitCombiPotential, z) = z2i(V.basis, z)
# i2z(V::FitCombiPotential, i) = i2z(V.basis, i)
# numz(V::FitCombiPotential) = length(zlist(V.basis))

fltype(V::FitCombiPotential{T, TP}) where {T, TP} = typeof( one(T) * one(TP) )

cutoff(V::FitCombiPotential) = cutoff(V.basis)

function FitCombiPotential(basis, F, numfeatures)
   T = fltype(basis)
   NZ = length(zlist(basis))
   params = ntuple( iz -> zeros(T, (numfeatures, length(basis, iz))), NZ )
   return FitCombiPotential(basis, params, F)
end

# -------------- Parameter management ...

num_features(V::FitCombiPotential) = size(V.params[1], 1)

get_params(V::FitCombiPotential) = vcat(vec.(V.params)...)

function set_params!(V::FitCombiPotential, θ)
   idx = 0
   len = length(V.params[1])
   for iz = 1:numz(V)
      V.params[iz][:] .= θ[(idx+1):(idx+len)]
      idx += len
   end
   return V
end

function set_params(V::FitCombiPotential, p::Vector{TP}) where {TP}
   new_params = ntuple( iz -> zeros(TP, (num_features(V), length(V.basis, iz))), numz(V) )
   Vnew = FitCombiPotential(V.basis, new_params, V.F)
   idx = 0
   len = length(Vnew.params[1])
   for iz = 1:numz(V)
      Vnew.params[iz][:] .= p[(idx+1):(idx+len)]
      idx += len
   end
   return Vnew
end

# -------------- Evaluation Code

function evaluate!(tmp, V::FitCombiPotential,
                   Rs::AbstractVector{JVec{T}},
                   Zs::AbstractVector{<:AtomicNumber},
                   z0::AtomicNumber) where {T}
   iz0 = z2i(V, z0)
   # Evaluate the inner basis
   #        TODO: replace with non-allocating version!
   B = evaluate(V.basis, Rs, Zs, z0)
   # only  get the part of the basis vector that matters to this site
   B_z0 = @view B[V.basis.Bz0inds[iz0]]
   # apply the parameters to get the densities
   ρ = V.params[iz0] * B_z0
   # return the nonlinear function of the densities.
   return V.F(ρ)
end

alloc_temp_d(V::FitCombiPotential, N::Integer) =
      (dV = zeros(JVec{fltype(V)}, N),
        R = zeros(JVecF, N),
        Z = zeros(AtomicNumber, N), )

function evaluate_d!(dEs, tmpd, V::FitCombiPotential,
                   Rs::AbstractVector{JVec{T}},
                   Zs::AbstractVector{<:AtomicNumber},
                   z0::AtomicNumber) where {T}
   @show eltype(dEs), fltype(V)
   iz0 = z2i(V, z0)
   # Evaluate the densities (cf evaluate! for comments)
   B = evaluate(V.basis, Rs, Zs, z0)
   B_z0 = @view B[V.basis.Bz0inds[iz0]]
   ρ = V.params[iz0] * B_z0
   # derivative of F ---  TODO : get rid of enforcing AD here!
   dF = ForwardDiff.gradient(V.F, ρ)
   # get the collapsed coefficients
   #    ∂_{rj} Ei = ∑_p ∂_{ρp} F * ∂_{rj} ρp
   #              = ∑_k [ ∑_p ∂_{ρp} F * θ_{pk} ] * ∂_{rj} B_k
   c = dF' * V.params[iz0]   # TODO: this is ridiculous ...
   dB = evaluate_d(V.basis, Rs, Zs, z0)
   dB_z0 = @view dB[V.basis.Bz0inds[iz0], :]
   dEi = (UniformScaling.(c) * dB_z0)[:]
   for n = 1:length(dEi)
      dEs[n] = dEi[n]
   end
   return dEs
end

# --------------- Parameter Gradient

# SECOND STEP: fast evaluator! -> TODO later
# mutable struct CombiPotential{TF} <: AbstractCalculator
#    ace::PIPotential
#    F::TF
# end

end


# ------------------------------------
# Secure the array for explicit and implicit
# ------------------------------------
function common_allocation(cellxmax, cellymax, cellzmax, nval)
    # define at cell center
    Qbase     = zeros(cellxmax, cellymax, cellzmax, nval)        # primitive variables
    Qbase_ave = zeros(cellxmax, cellymax, cellzmax, nval)        # average primitive variables
    volume    = zeros(cellxmax, cellymax, cellzmax)              # volume

    cellcenter = zeros(cellxmax, cellymax, cellzmax, 3)          # cell center

    wally = zeros(cellxmax, cellymax, cellzmax)                  # dis from wall
    yplus =  ones(cellxmax, cellymax, cellzmax) * 1e5            # yplus

    Qcon     = zeros(cellxmax, cellymax, cellzmax, nval)         # conserved variables
    Qcon_hat = zeros(cellxmax, cellymax, cellzmax, nval)         # conserved variables in the general coordinate system

    QbaseU = zeros(cellxmax+1, cellymax, cellzmax, nval)        # Q up
    QbaseD = zeros(cellxmax+1, cellymax, cellzmax, nval)        # Q down
    QbaseL = zeros(cellxmax, cellymax+1, cellzmax, nval)        # Q left
    QbaseR = zeros(cellxmax, cellymax+1, cellzmax, nval)        # Q right
    QbaseF = zeros(cellxmax, cellymax, cellzmax+1, nval)        # Q front
    QbaseB = zeros(cellxmax, cellymax, cellzmax+1, nval)        # Q back

    QconU = zeros(cellxmax+1, cellymax, cellzmax, nval)        # Q up
    QconD = zeros(cellxmax+1, cellymax, cellzmax, nval)        # Q down
    QconL = zeros(cellxmax, cellymax+1, cellzmax, nval)        # Q left
    QconR = zeros(cellxmax, cellymax+1, cellzmax, nval)        # Q right
    QconF = zeros(cellxmax, cellymax, cellzmax+1, nval)        # Q front
    QconB = zeros(cellxmax, cellymax, cellzmax+1, nval)        # Q back

    mu     = zeros(cellxmax, cellymax, cellzmax)                 # viscosity
    lambda = zeros(cellxmax, cellymax, cellzmax)                 # thermal Conductivity
    mut    = zeros(cellxmax, cellymax, cellzmax)                 # turb viscosity
    mut_bd = zeros(cellxmax+1, cellymax+1, cellzmax+1, 3)        # turb viscosity on bd

    RHS = zeros(cellxmax, cellymax, cellzmax, nval)              # right hand side

    E_adv_hat = zeros(cellxmax+1,   cellymax,   cellzmax, nval)    # flux of advection in the x-direction
    F_adv_hat = zeros(  cellxmax, cellymax+1,   cellzmax, nval)    # flux of advection in the y-direction
    G_adv_hat = zeros(  cellxmax,   cellymax, cellzmax+1, nval)    # flux of advection in the z-direction

    E_vis_hat = zeros(cellxmax+1,   cellymax,   cellzmax, nval)    # flux of viscosity in the x-direction
    F_vis_hat = zeros(  cellxmax, cellymax+1,   cellzmax, nval)    # flux of viscosity in the y-direction
    G_vis_hat = zeros(  cellxmax,   cellymax, cellzmax+1, nval)    # flux of viscosity in the z-direction

    return Qbase, Qbase_ave, volume, cellcenter, wally, yplus, Qcon, Qcon_hat, mu, mut, mut_bd, lambda,
            E_adv_hat, F_adv_hat, G_adv_hat, E_vis_hat, F_vis_hat, G_vis_hat, RHS, QbaseU, QbaseD, QbaseL, QbaseR, QbaseF, QbaseB,
            QconU, QconD, QconL, QconR, QconF, QconB

end

function allocation_implicit(cellxmax, cellymax, cellzmax, nval)
    # define at cell center
    Qbasen     = zeros(cellxmax, cellymax, cellzmax, nval)        # primitive variables for inner iteration
    Qconn      = zeros(cellxmax, cellymax, cellzmax, nval)        # conserved variables for inner iteration
    Qconn_hat  = zeros(cellxmax, cellymax, cellzmax, nval)        # conserved variables in general coordinate system for inner iteration
    Qbasem     = zeros(cellxmax, cellymax, cellzmax, nval)        # primitive variables for inner iteration

    dtau         = zeros(cellxmax, cellymax, cellzmax)            # computational time steps

    A_adv_hat_p = zeros(cellxmax, cellymax, cellzmax, nval, nval) # Jacobian matrix A+ for one-wave approximation
    A_adv_hat_m = zeros(cellxmax, cellymax, cellzmax, nval, nval) # Jacobian matrix A- for one-wave approximation
    B_adv_hat_p = zeros(cellxmax, cellymax, cellzmax, nval, nval) # Jacobian matrix B+ for one-wave approximation
    B_adv_hat_m = zeros(cellxmax, cellymax, cellzmax, nval, nval) # Jacobian matrix B- for one-wave approximation
    C_adv_hat_p = zeros(cellxmax, cellymax, cellzmax, nval, nval) # Jacobian matrix C+ for one-wave approximation
    C_adv_hat_m = zeros(cellxmax, cellymax, cellzmax, nval, nval) # Jacobian matrix C- for one-wave approximation
    A_beta_shig = zeros(cellxmax, cellymax, cellzmax)             # A sigma for one-wave approximation
    B_beta_shig = zeros(cellxmax, cellymax, cellzmax)             # B sigma for one-wave approximation
    C_beta_shig = zeros(cellxmax, cellymax, cellzmax)             # C sigma for one-wave approximation

    jalphaP = zeros(cellxmax, cellymax, cellzmax)                 # Jacobian matrix for viscosity
    jbetaP  = zeros(cellxmax, cellymax, cellzmax)                 # Jacobian matrix for viscosity
    jgammaP = zeros(cellxmax, cellymax, cellzmax)                 # Jacobian matrix for viscosity

    delta_Q      = zeros(cellxmax, cellymax, cellzmax, nval)      # delta Q for lusgs
    delta_Q_temp = zeros(cellxmax, cellymax, cellzmax, nval)      # temporary delta Q for lusgs

    D  = zeros(cellxmax, cellymax, cellzmax)                      # Diagonal of LHS
    Lx = zeros(cellxmax, cellymax, cellzmax, nval, nval)          # lower of LHS
    Ly = zeros(cellxmax, cellymax, cellzmax, nval, nval)          # lower of LHS
    Lz = zeros(cellxmax, cellymax, cellzmax, nval, nval)          # lower of LHS
    Ux = zeros(cellxmax, cellymax, cellzmax, nval, nval)          # upper of LHS
    Uy = zeros(cellxmax, cellymax, cellzmax, nval, nval)          # upper of LHS
    Uz = zeros(cellxmax, cellymax, cellzmax, nval, nval)          # upper of LHS

    LdQ = zeros(cellxmax, cellymax, cellzmax, nval)               # lower of LHS
    UdQ = zeros(cellxmax, cellymax, cellzmax, nval)               # upper of LHS

    RHS_temp = zeros(cellxmax, cellymax, cellzmax, nval)          # temporary RHS
    res = zeros(cellxmax, cellymax, cellzmax, nval)               # residual

    # define at cell boundaries
    lambda_facex = zeros(cellxmax+1, cellymax, cellzmax)          # lambda for computational time steps
    lambda_facey = zeros(cellxmax, cellymax+1, cellzmax)          # lambda for computational time steps
    lambda_facez = zeros(cellxmax, cellymax, cellzmax+1)          # lambda for computational time steps

    # define at cell boundaries
    dx = zeros(cellxmax+1, cellymax, cellzmax)                   # distance from cell center
    dy = zeros(cellxmax, cellymax+1, cellzmax)                   # distance from cell center
    dz = zeros(cellxmax, cellymax, cellzmax+1)                   # distance from cell center

    # misc
    norm2 = zeros(nval)                                 # Residuals by norm-2
    I = zeros(nval, nval)                               # identity matrix
    for l in 1:nval
        I[l,l] = 1.0
    end

    return Qbasen, Qconn, Qconn_hat, Qbasem, dtau, lambda_facex, lambda_facey, lambda_facez, dx, dy, dz,
            A_adv_hat_m, A_adv_hat_p, B_adv_hat_m, B_adv_hat_p, C_adv_hat_p, C_adv_hat_m, A_beta_shig, B_beta_shig, C_beta_shig,
            jalphaP, jbetaP, jgammaP, delta_Q, delta_Q_temp, D, Lx, Ly, Lz, Ux, Uy, Uz, LdQ, UdQ, RHS_temp, res,
            norm2, I
end


sbmlfile = joinpath("data", "ecoli_core.xml")
download_data_file(
    "http://systemsbiology.ucsd.edu/sites/systemsbiology.ucsd.edu/files/Attachments/Images/downloads/Ecoli_core/ecoli_core_model.xml",
    sbmlfile,
    "78692f8509fb36534f4f9b6ade23b23552044f3ecd8b48d84d484636922ae907",
)

@testset "SBML import and conversion" begin
    sbmlm = load_sbml_model(sbmlfile)
    m = convert(CoreModel, sbmlm)

    @test size(stoichiometry(sbmlm)) == (92, 95)
    @test size(stoichiometry(m)) == (n_metabolites(sbmlm), n_reactions(sbmlm))
    @test length(m.S.nzval) == 380
    @test length.(bounds(sbmlm)) == (95, 95)
    @test length.(bounds(m)) == (95, 95)
    @test all([length(m.xl), length(m.xu), length(m.c)] .== 95)

    @test metabolites(m)[1:3] == ["M_succoa_c", "M_ac_c", "M_fru_b"]
    @test reactions(m)[1:3] == ["R_EX_fum_e", "R_ACONTb", "R_GLNS"]

    cm = convert(CoreModelCoupled, sbmlm)
    @test n_coupling_constraints(cm) == 0
end


function add_re!(sys)
    re = RenewableDispatch(
        "WindBusA",
        true,
        get_component(Bus, sys, "bus5"),
        0.0,
        0.0,
        1.200,
        PrimeMovers.WT,
        (min = 0.0, max = 0.0),
        1.0,
        TwoPartCost(0.220, 0.0),
        100.0,
    )
    add_component!(sys, re)
    copy_time_series!(re, get_component(PowerLoad, sys, "bus2"))

    fx = RenewableFix(
        "RoofTopSolar",
        true,
        get_component(Bus, sys, "bus5"),
        0.0,
        0.0,
        1.100,
        PrimeMovers.PVe,
        1.0,
        10.0,
    )
    add_component!(sys, fx)
    copy_time_series!(fx, get_component(PowerLoad, sys, "bus2"))

    for g in get_components(HydroEnergyReservoir, sys)
        tpc = get_operation_cost(g)
        smc = StorageManagementCost(
            variable = get_variable(tpc),
            fixed = get_fixed(tpc),
            start_up = 0.0,
            shut_down = 0.0,
            energy_shortage_cost = 10.0,
            energy_surplus_cost = 10.0,
        )
        set_operation_cost!(g, smc)
    end

    batt = GenericBattery(
        "test_batt",
        true,
        get_component(Bus, sys, "bus4"),
        PrimeMovers.BA,
        0.0,
        (min = 0.0, max = 1.0),
        1.0,
        0.0,
        (min = 0.0, max = 1.0),
        (min = 0.0, max = 1.0),
        (in = 1.0, out = 1.0),
        0.0,
        nothing,
        10.0,
    )
    add_component!(sys, batt)
end

function run_test_sim(result_dir::String)
    sim_name = "results_sim"
    sim_path = joinpath(result_dir, sim_name)

    if ispath(sim_path)
        @info "Reading UC system from" sim_path
        c_sys5_hy_uc = System(
            joinpath(sim_path, "..", "c_sys5_hy_uc.json"),
            time_series_read_only = true,
        )
        @info "Reading ED system from" sim_path
        c_sys5_hy_ed = System(
            joinpath(sim_path, "..", "c_sys5_hy_ed.json"),
            time_series_read_only = true,
        )
        executions = tryparse.(Int, readdir(sim_path))
        sim = joinpath(sim_path, string(maximum(executions)))
        @info "Reading results from last execution" sim
    else
        @info "Building UC system from"
        c_sys5_hy_uc = PSB.build_system(PSB.SIIPExampleSystems, "5_bus_hydro_uc_sys")
        @info "Building ED system from"
        c_sys5_hy_ed = PSB.build_system(PSB.SIIPExampleSystems, "5_bus_hydro_ed_sys")

        @info "Adding extra RE"
        add_re!(c_sys5_hy_uc)
        add_re!(c_sys5_hy_ed)
        to_json(c_sys5_hy_uc, joinpath(sim_path, "..", "c_sys5_hy_uc.json"), force = true)
        to_json(c_sys5_hy_ed, joinpath(sim_path, "..", "c_sys5_hy_ed.json"), force = true)

        mkpath(result_dir)
        GLPK_optimizer =
            optimizer_with_attributes(GLPK.Optimizer, "msg_lev" => GLPK.GLP_MSG_OFF)

        template_hydro_st_uc = template_unit_commitment()
        set_device_model!(template_hydro_st_uc, HydroDispatch, FixedOutput)
        set_device_model!(
            template_hydro_st_uc,
            HydroEnergyReservoir,
            HydroDispatchReservoirStorage,
        )
        set_device_model!(template_hydro_st_uc, GenericBattery, BookKeeping)

        template_hydro_st_ed = template_economic_dispatch(
            network = CopperPlatePowerModel,
            use_slacks = true,
            duals = [CopperPlateBalanceConstraint],
        )
        set_device_model!(template_hydro_st_ed, HydroDispatch, FixedOutput)
        set_device_model!(
            template_hydro_st_ed,
            HydroEnergyReservoir,
            HydroDispatchReservoirStorage,
        )
        set_device_model!(template_hydro_st_ed, GenericBattery, BookKeeping)
        template_hydro_st_ed.services = Dict() #remove ed services
        models = SimulationModels(
            decision_models = [
                DecisionModel(
                    template_hydro_st_uc,
                    c_sys5_hy_uc,
                    optimizer = GLPK_optimizer,
                    name = "UC",
                    system_to_file = false,
                ),
                DecisionModel(
                    template_hydro_st_ed,
                    c_sys5_hy_ed,
                    optimizer = GLPK_optimizer,
                    name = "ED",
                    system_to_file = false,
                ),
            ],
        )

        sequence = SimulationSequence(
            models = models,
            feedforwards = feedforward = Dict(
                "ED" => [
                    SemiContinuousFeedforward(
                        component_type = ThermalStandard,
                        source = OnVariable,
                        affected_values = [ActivePowerVariable],
                    ),
                    EnergyLimitFeedforward(
                        component_type = HydroEnergyReservoir,
                        source = ActivePowerVariable,
                        affected_values = [ActivePowerVariable],
                        number_of_periods = 12,
                    ),
                ],
            ),
            ini_cond_chronology = InterProblemChronology(),
        )
        sim = Simulation(
            name = "results_sim",
            steps = 2,
            models = models,
            sequence = sequence,
            simulation_folder = result_dir,
        )
        build!(sim)
        execute!(sim)
    end

    results = SimulationResults(sim)
    results_uc = get_decision_problem_results(results, "UC")
    set_system!(results_uc, c_sys5_hy_uc)
    results_ed = get_decision_problem_results(results, "ED")
    set_system!(results_ed, c_sys5_hy_ed)

    return results_uc, results_ed
end

function run_test_prob()
    c_sys5_hy_uc = PSB.build_system(PSB.SIIPExampleSystems, "5_bus_hydro_uc_sys")
    add_re!(c_sys5_hy_uc)
    GLPK_optimizer =
        optimizer_with_attributes(GLPK.Optimizer, "msg_lev" => GLPK.GLP_MSG_OFF)

    template_hydro_st_uc = template_unit_commitment()
    set_device_model!(template_hydro_st_uc, HydroDispatch, FixedOutput)
    set_device_model!(
        template_hydro_st_uc,
        HydroEnergyReservoir,
        HydroDispatchReservoirStorage,
    )

    prob = DecisionModel(
        template_hydro_st_uc,
        c_sys5_hy_uc,
        optimizer = GLPK_optimizer,
        horizon = 12,
    )
    build!(prob, output_dir = mktempdir())
    solve!(prob)
    res = ProblemResults(prob)
    return res
end


module Model

using Distributions

# TODO: Remove this dependency
using RCall # Mclust

import LinearAlgebra # Identity matrix
import Random # shuffle
import StatsBase # wsample, counts

include("../MCMC/MCMC.jl")
import .MCMC.Util.@namedargs

include("util.jl")
include("DICparam.jl")
include("State.jl")
include("Data.jl")
include("Constants.jl")
include("Tuners.jl")
include("repFAM/repFAM.jl")
include("update.jl")
include("FeatureSelect/FeatureSelect.jl")
include("genInitialState.jl")
include("fit.jl")

#=
precompile(cytof5_fit, (State, Constants, Data, Int, Int,
                        Vector{Vector{Symbol}}, Vector{Int}, Bool, Int, Bool,
                        Bool, Bool))
=#

end # Model


# Affine decision function #
# ========================== #
struct AffineDecisionObjectiveBridge{T} <: MOIB.Objective.AbstractBridge
    decision_function::AffineDecisionFunction{T}
end

function MOIB.Objective.bridge_objective(::Type{AffineDecisionObjectiveBridge{T}}, model::MOI.ModelLike,
                                         f::AffineDecisionFunction{T}) where T
    # All decisions have been mapped to either the decision part constant or the variable part terms
    # at this point.
    F = MOI.ScalarAffineFunction{T}
    # Set the bridged objective
    MOI.set(model, MOI.ObjectiveFunction{F}(), MOI.ScalarAffineFunction(f.variable_part.terms, zero(T)))
    # Save decision function to allow modifications
    return AffineDecisionObjectiveBridge{T}(f)
end

function MOIB.Objective.supports_objective_function(
    ::Type{<:AffineDecisionObjectiveBridge}, ::Type{<:AffineDecisionFunction})
    return true
end
MOIB.added_constrained_variable_types(::Type{<:AffineDecisionObjectiveBridge}) = Tuple{DataType}[]
function MOIB.added_constraint_types(::Type{<:AffineDecisionObjectiveBridge})
    return Tuple{DataType, DataType}[]
end
function MOIB.set_objective_function_type(::Type{AffineDecisionObjectiveBridge{T}}) where T
    return MOI.ScalarAffineFunction{T}
end

function MOI.get(::AffineDecisionObjectiveBridge, ::MOI.NumberOfVariables)
    return 0
end

function MOI.get(::AffineDecisionObjectiveBridge, ::MOI.ListOfVariableIndices)
    return MOI.VariableIndex[]
end

function MOI.delete(::MOI.ModelLike, ::AffineDecisionObjectiveBridge)
    # Nothing to delete
    return nothing
end

function MOI.set(::MOI.ModelLike, ::MOI.ObjectiveSense,
                 ::AffineDecisionObjectiveBridge, ::MOI.OptimizationSense)
    # Nothing to handle if sense changes
    return nothing
end

function MOI.get(model::MOI.ModelLike,
                 attr::MOIB.ObjectiveFunctionValue{F},
                 bridge::AffineDecisionObjectiveBridge{T}) where {T, F <: AffineDecisionFunction{T}}
    f = bridge.decision_function
    G = MOI.ScalarAffineFunction{T}
    obj_val = MOI.get(model, MOIB.ObjectiveFunctionValue{G}(attr.result_index))
    # Calculate and add constant
    constant = f.variable_part.constant +
        f.known_part.constant
    return obj_val + constant
end

function MOI.get(model::MOI.ModelLike, attr::MOI.ObjectiveFunction{F},
                 bridge::AffineDecisionObjectiveBridge{T}) where {T, F <: AffineDecisionFunction{T}}
    f = bridge.decision_function
    # Remove mapped variables to properly unbridge
    from_decision(v) = begin
        result = any(t -> t.variable_index == v, f.decision_part.terms)
    end
    g = AffineDecisionFunction(
        MOIU.filter_variables(v -> !from_decision(v), f.variable_part),
        copy(f.decision_part),
        copy(f.known_part))
    return g
end

function MOI.modify(model::MOI.ModelLike, bridge::AffineDecisionObjectiveBridge{T}, change::MOI.ScalarConstantChange) where T
    f = bridge.decision_function
    # Modify constant of variable part
    f.variable_part.constant = change.new_constant
    return nothing
end

function MOI.modify(model::MOI.ModelLike, bridge::AffineDecisionObjectiveBridge{T}, change::MOI.ScalarCoefficientChange) where T
    f = bridge.decision_function
    # Modify variable part of decision function
    modify_coefficient!(f.variable_part.terms, change.variable, change.new_coefficient)
    # Modify the variable part of the mapped objective as well
    F = MOI.ScalarAffineFunction{T}
    MOI.modify(model, MOI.ObjectiveFunction{F}(), change)
    return nothing
end

function MOI.modify(model::MOI.ModelLike, bridge::AffineDecisionObjectiveBridge{T}, change::DecisionCoefficientChange) where T
    f = bridge.decision_function
    # Update the decision coefficient
    modify_coefficient!(f.decision_part.terms, change.decision, change.new_coefficient)
    # Update mapped variable through ScalarCoefficientChange
    MOI.modify(model, bridge, MOI.ScalarCoefficientChange(change.decision, change.new_coefficient))
    return nothing
end

function MOI.modify(model::MOI.ModelLike, bridge::AffineDecisionObjectiveBridge{T}, change::KnownCoefficientChange) where T
    f = bridge.decision_function
    i = something(findfirst(t -> t.variable_index == change.known,
                            f.known_part.terms), 0)
    # Update known part of objective constant
    coefficient = iszero(i) ? zero(T) : f.known_part.terms[i].coefficient
    known_value = MOI.get(model, MOI.VariablePrimal(), change.known)
    f.known_part.constant +=
        (change.new_coefficient - coefficient) * known_value
    # Update the decision coefficient
    modify_coefficient!(f.known_part.terms, change.known, change.new_coefficient)
    return nothing
end

function MOI.modify(model::MOI.ModelLike, bridge::AffineDecisionObjectiveBridge{T}, change::KnownValueChange) where T
    f = bridge.decision_function
    i = something(findfirst(t -> t.variable_index == change.known,
                            f.known_part.terms), 0)
    if iszero(i)
        # Known value not in objective, nothing to do
        return nothing
    end
    # Update known part of objective constant
    coefficient = f.known_part.terms[i].coefficient
    f.known_part.constant +=
        coefficient * change.value_difference
    return nothing
end

function MOI.modify(model::MOI.ModelLike, bridge::AffineDecisionObjectiveBridge{T}, change::KnownValuesChange) where T
    f = bridge.decision_function
    # Update known value
    known_val = zero(T)
    for term in f.known_part.terms
        known_val += term.coefficient * MOI.get(model, MOI.VariablePrimal(), term.variable_index)
    end
    # Update known part of objective constant
    f.known_part.constant = known_val
    return nothing
end


@inline remove_column(table::TypedTables.Table, colname::Symbol) =
    TypedTables.Table(Base.structdiff(TypedTables.columns(table), NamedTuple{(colname,),Tuple{Int}}(0)))

@inline add_column(table::TypedTables.Table, nt::NamedTuple) =
    TypedTables.Table(merge(nt, TypedTables.columns(table)))
@inline add_column(table::TypedTables.Table, newname::Symbol, col::CT) where {CT} =
    add_column(table, NamedTuple{(newname,), Tuple{CT}}((col,)) )

@inline _rename_fields_impl(nt::NamedTuple{names}, from::Val{a}, to::Val{b}) where {names, a, b} =
    NamedTuple{map(x -> x == a ? b : x, names)}(values(nt))

@inline rename_fields(nt::NamedTuple, pattern::Pair{Symbol,Symbol}) =
    _rename_fields_impl(nt, Val(pattern.first), Val(pattern.second))

@inline rename_cols(table::Any, pattern::Pair{Symbol,Symbol}) =
    TypedTables.Tables.materializer(table)(rename_fields(TypedTables.Tables.columns(table), pattern))

function common_length(x, y)
    a = length(x); b = length(y)
    if a == b || b == 1
        a
    elseif a == 1
        b
    else
        throw(ArgumentError("Lengths are not compatible"))
    end
end
common_length(x, y, z...) = common_length(common_length(x, y), z...)

nested_col(x::AbstractVector, n::AbstractVector{<:Integer}) = VectorOfVectors(Fill.(x, n))

function nested_col(x::AbstractVector{<:AbstractVector}, n::AbstractVector{<:Integer})
    @assert length.(x) == n
    x
end

function blow_up_to_commen_length(table::TypedTables.Table)
    n = common_length.(values(TypedTables.columns(table))...)
    return TypedTables.Table(map(c -> nested_col(c, n), TypedTables.columns(table)))
end

function split_table_by_each_charge_deposition(table::TypedTables.Table)
    row_lengths = length.(values(table[1]))
    all_vectors = all(t -> t <: AbstractVector, typeof.(values(table[1])))
    if all(isequal(row_lengths[1]), row_lengths) && all_vectors
        TypedTables.Table( map(flatview, TypedTables.columns(table)) )
    else
        split_table_by_each_charge_deposition(blow_up_to_commen_length(table))
    end
end

function sort_by_time(table::TypedTables.Table)
    @assert length(table.thit[1]) == 1 "Split the table first: split_table_by_each_charge_deposition"
    sorted_inds = sortperm(table.thit)
    TypedTables.Table(map( c -> c[sorted_inds], TypedTables.columns(table)))
end

function cluster_by_time(table::TypedTables.Table, Δt = 1u"ns")
    @assert length(table.thit[1]) == 1 "Split the table first: split_table_by_each_charge_deposition"
    if Δt == 0u"s" return table end
    times_sorted = !issorted(table.thit) ? sort_by_time(table).thit : table.thit
    n_charge_depos = length(times_sorted)
    t0 = times_sorted[1]
    t_last = t0
    inds = Int[]
    for i in 2:n_charge_depos
        t = times_sorted[i]
        dt = t - t_last
        if dt > Δt
            push!(inds, i - 1)
            t_last = t
        end
    end
    if length(inds) > 0
        ranges = vcat( [1:inds[1]], broadcast(:, inds[1:end-1].+1, inds[2:end]),
                        n_charge_depos > inds[end] ? [inds[end]+1:n_charge_depos] : [] )
        TypedTables.Table(map(c -> VectorOfVectors(map(r -> c[r], ranges)), TypedTables.columns(table)))
    else
        TypedTables.Table(map(c -> VectorOfVectors(map(r -> c[r], [1:n_charge_depos])), TypedTables.columns(table)))
    end
end

function add_physics_event_numbers(table::TypedTables.Table)
    n_events = length(TypedTables.columns(table)[1])
    add_column(table, :phyevtno, flatview(collect(Int32, 1:n_events)))
end

function groub_by_evt_number(table::TypedTables.Table, colname::Symbol = :mcevtno)
    sorting_idxs = sortperm(getproperty(table, colname))
    sorted = table[sorting_idxs]
    return TypedTables.Table(consgroupedview((getproperty(table, colname), Tables.columns(sorted))))
end

function translate_event_positions(table::TypedTables.Table, tv::AbstractVector{<:Unitful.Quantity{<:Real, Unitful.𝐋}})
    @assert length(tv) == 3 "translation vector must be of length 3"
    TP = eltype(eltype(eltype(table.pos)))
    u = unit(TP)
    @assert all(e -> dimension(e) == Unitful.𝐋, tv) "All elements of translation vector muss be of dimension Unitful.𝐋"
    trans = uconvert.(u, tv)
    newpos = (pos = VectorOfVectors(map(poss -> map(pos -> pos + trans, poss), table.pos)),)
    TypedTables.Table(merge( TypedTables.columns(table), newpos ))
end

function rotate_event_positions(table, rot::Rotations.Rotation{3})
    newpos = (pos = VectorOfVectors(map(poss -> map(pos -> rot * pos, poss), table.pos)),)
    TypedTables.Table(merge( TypedTables.columns(table), newpos ))
end

function chunked_ranges(n, chunksize)
    if n < chunksize return [1:n] end
    ranges = map( s -> s:s+chunksize-1, 1:chunksize:n)
    if last(ranges[end]) > n ranges[end] = first(ranges[end]):n end
    return ranges
end


function tvcred_parameterize!(m::AbstractDSGEModel)
    # Parameterize
    for i in 1:5
        adj = (i == 2 || i == 3) ? 0.25 : 1.
        ModelConstructors.set_regime_val!(m[:σ_g], i, adj * m[:σ_g].value)
        ModelConstructors.set_regime_val!(m[:σ_b], i, adj * m[:σ_b].value)
        ModelConstructors.set_regime_val!(m[:σ_μ], i, adj * m[:σ_μ].value)
        ModelConstructors.set_regime_val!(m[:σ_ztil], i, adj * m[:σ_ztil].value)
        ModelConstructors.set_regime_val!(m[:σ_λ_f], i, adj * m[:σ_λ_f].value)
        ModelConstructors.set_regime_val!(m[:σ_λ_w], i, adj * m[:σ_λ_w].value)
        ModelConstructors.set_regime_val!(m[:σ_r_m], i, adj * m[:σ_r_m].value)
        ModelConstructors.set_regime_val!(m[:σ_σ_ω], i, adj * m[:σ_σ_ω].value)
        ModelConstructors.set_regime_val!(m[:σ_μ_e], i, adj * m[:σ_μ_e].value)
        ModelConstructors.set_regime_val!(m[:σ_γ], i, adj * m[:σ_γ].value)
        ModelConstructors.set_regime_val!(m[:σ_π_star], i, adj * m[:σ_π_star].value)
        ModelConstructors.set_regime_val!(m[:σ_lr], i, adj * m[:σ_lr].value)
        ModelConstructors.set_regime_val!(m[:σ_z_p], i, adj * m[:σ_z_p].value)
        ModelConstructors.set_regime_val!(m[:σ_tfp], i, adj * m[:σ_tfp].value)
        err_adj = (i in 2:4) ? 10. : adj
        ModelConstructors.set_regime_val!(m[:σ_gdpdef], i, err_adj * m[:σ_gdpdef].value)
        ModelConstructors.set_regime_val!(m[:σ_corepce], i, err_adj * m[:σ_corepce].value)
        ModelConstructors.set_regime_val!(m[:σ_gdp], i, adj * m[:σ_gdp].value)
        ModelConstructors.set_regime_val!(m[:σ_gdi], i, adj * m[:σ_gdi].value)

        for j = 1:DSGE.n_mon_anticipated_shocks(m)
            ModelConstructors.set_regime_val!(m[Symbol("σ_r_m$(j)")], i, adj * m[Symbol("σ_r_m$(j)")])
        end
    end

    ModelConstructors.set_regime_val!(m[:σ_z_p], 2, 0.)
    ModelConstructors.set_regime_val!(m[:σ_z_p], 3, 0.)
    ModelConstructors.set_regime_val!(m[:σ_r_m], 2, 10. * m[:σ_r_m].value)
    ModelConstructors.set_regime_val!(m[:σ_r_m], 3, 10. * m[:σ_r_m].value)
    for i = 2:3
        for j = 1:DSGE.n_mon_anticipated_shocks(m)
            ModelConstructors.set_regime_val!(m[Symbol("σ_r_m$(j)")], i, m[Symbol("σ_r_m$(j)")].value)
        end
    end

    # Set non-standard shocks
    ModelConstructors.set_regime_val!(m[:σ_φ], 1, 0.)
    ModelConstructors.set_regime_val!(m[:σ_φ], 2, 400.)
    ModelConstructors.set_regime_val!(m[:σ_φ], 3, 400.)
    ModelConstructors.set_regime_val!(m[:σ_φ], 4, 400.)
    ModelConstructors.set_regime_val!(m[:σ_φ], 5, 0.)
    ModelConstructors.set_regime_val!(m[:σ_φ_prop], 1, 0.)
    ModelConstructors.set_regime_val!(m[:σ_φ_prop], 2, 2.)
    ModelConstructors.set_regime_val!(m[:σ_φ_prop], 3, 0.)
    ModelConstructors.set_regime_val!(m[:σ_φ_prop], 4, 0.)
    ModelConstructors.set_regime_val!(m[:σ_φ_prop], 5, 0.)
    ModelConstructors.set_regime_val!(m[:σ_biidc], 1, 0.)
    ModelConstructors.set_regime_val!(m[:σ_biidc], 2, 4.)
    ModelConstructors.set_regime_val!(m[:σ_biidc], 3, 4.)
    ModelConstructors.set_regime_val!(m[:σ_biidc], 4, 4.)
    ModelConstructors.set_regime_val!(m[:σ_biidc], 5, 0.)
    ModelConstructors.set_regime_val!(m[:σ_biidc_prop], 1, 0.)
    ModelConstructors.set_regime_val!(m[:σ_biidc_prop], 2, 2.)
    ModelConstructors.set_regime_val!(m[:σ_biidc_prop], 3, 0.)
    ModelConstructors.set_regime_val!(m[:σ_biidc_prop], 4, 0.)
    ModelConstructors.set_regime_val!(m[:σ_biidc_prop], 5, 0.)
    ModelConstructors.set_regime_val!(m[:σ_ygap], 1, 0.)
    ModelConstructors.set_regime_val!(m[:σ_ygap], 2, 0.)
    ModelConstructors.set_regime_val!(m[:σ_ygap], 3, 20.)
    ModelConstructors.set_regime_val!(m[:σ_ygap], 4, 0.)
    ModelConstructors.set_regime_val!(m[:σ_ygap], 5, 0.)
    ModelConstructors.set_regime_val!(m[:σ_pgap], 1, 0.)
    ModelConstructors.set_regime_val!(m[:σ_pgap], 2, 0.)
    ModelConstructors.set_regime_val!(m[:σ_pgap], 3, 20.)
    ModelConstructors.set_regime_val!(m[:σ_pgap], 4, 0.)
    ModelConstructors.set_regime_val!(m[:σ_pgap], 5, 0.)
    ModelConstructors.set_regime_val!(m[:σ_ziid], 1, 0.)
    ModelConstructors.set_regime_val!(m[:σ_ziid], 2, 5.)
    ModelConstructors.set_regime_val!(m[:σ_ziid], 3, 5.)
    ModelConstructors.set_regime_val!(m[:σ_ziid], 4, 5.)
    ModelConstructors.set_regime_val!(m[:σ_ziid], 5, 0.)
    ModelConstructors.set_regime_val!(m[:σ_ziid_prop], 1, 0.)
    ModelConstructors.set_regime_val!(m[:σ_ziid_prop], 2, 2.)
    ModelConstructors.set_regime_val!(m[:σ_ziid_prop], 3, 0.)
    ModelConstructors.set_regime_val!(m[:σ_ziid_prop], 4, 0.)
    ModelConstructors.set_regime_val!(m[:σ_ziid_prop], 5, 0.)
    m[:σ_biidc1].prior = m[:σ_biidc].prior
    m[:σ_biidc1].valuebounds = m[:σ_biidc].valuebounds
    ModelConstructors.set_regime_val!(m[:σ_biidc1], 1, 0.)
    ModelConstructors.set_regime_val!(m[:σ_biidc1], 2, 0.)
    ModelConstructors.set_regime_val!(m[:σ_biidc1], 3, 0.)
    ModelConstructors.set_regime_val!(m[:σ_biidc1], 4, ModelConstructors.regime_val(m[:σ_biidc], 2))
    ModelConstructors.set_regime_val!(m[:σ_biidc1], 5, 0.)
end

function set_regime_vals_fnct(m, n)
    if n > 6
        for p in m.parameters
            regime_switching = !isempty(p.regimes)
            if regime_switching
                if haskey(p.regimes, :value)
                    for i in 6:n
                        ModelConstructors.set_regime_val!(p, i, ModelConstructors.regime_val(p, 1))
                    end
                end
            end
        end
    end
end


function mcmcmodel(panel::AbstractDataFrame,  zs::ZSpec,::Val{:randomeffects},
    ::Type{T}=PARAM[:mcmctype],
    ::Type{TV}=PARAM[:mcmcgpu] ? CUDA.CuVector{T} : Vector{T},
    ::Type{TM}=PARAM[:mcmcgpu] ? CUDA.CuMatrix{T} : Matrix{T},
    prior::Symbol=PARAM[:mcmcprior],
    Nburn::Int=PARAM[:mcmcnburn],
    Ndraws::Int=PARAM[:mcmcndraws]
    ) where {
      T<:Real, TV<:AbstractVector{T}, TM<:AbstractMatrix{T}}
  @assert PARAM[:predictiontype] ∈ [:leveldemean, :level]
  (T == Float64) || @warn("T=$T, which may lead to numerical instability.
    Is this really what you want?")
  CUDA.allowscalar(false)

  Zms::MeasureSpec = zs.Zms
  Fw::Vector{Symbol} = Fweights(Zms, :Fw)
  FRLw::Vector{Symbol} = Fweights(Zms, :FRLw)
  essentialfields = [Fw; FRLw; Zms.Fvol; ]

  #form the data into a more usable format and expand the arrays
  p = modelparts(panel, zs,T,TV,TM,
    weightdata = (Fw=Fw, FRLw=FRLw),
    Fvol=Zms.Fvol)
  dims = p.dims
  tidx::Dict = p.tidx
  cleanpanel::SubDataFrame = p.cleanpanel
  Ncleanpanel::Int = nrow(cleanpanel)
  ws::TM = p.dat[:Fw]
  RLws::TM = p.dat[:FRLw]
  ts::Vector{Int} = p.ts
  Ascale::Vector{T} = p.Ascale

  v::TV = p.dat[:Fvol]

  #println("ts: $ts")
  Xv = XYIter(ws, RLws, v, ts, Ascale)

  #construct the main MCMC object for book keeping and parameters
  hyper=PARAM[prior]
  initial=(
    A₀ = ones(T, dims.K),
    G = ones(T, dims.K, dims.T-1),
    σ² = ones(T, dims.T-1),
    ω = zeros(T, dims.T-1)
    )
  cc = CapacityChains(T, Nburn=Nburn, Ndraws=Ndraws, paramstemplate=initial, hyper=hyper)
  #construct the parameter
  Θ = VolumePartsMCMC(TM, TV, T; initial, cc, Xv, ts)
  #printmln(Θ.Ψ)
  createmcmc(Θ)

  #for debugging purposes- will trhow an error at some point if true
  PARAM[:runstacktraceonmcmc] #=&& solveA!(iterloss,Θ,Xv,_Xv, SolveType)

  #main optimization routine
  try
    λ⁺ = solveA!(iterloss,Θ,Xv,_Xv, SolveType)
  catch err
    errmessage = "$err"
    if length(errmessage) > 20_000 #sometimes we get really long error messages
      errmessage = errmessage[1:20_000]
    end
    @warn "Solver failed with error $errmessage"
  finally
    results::DataFrame = formresults(panel, Zms, Θ, Val{:iter}())

    #recompute the loss if needed
    if λ⁺<0
      updateA₁!(Θ.g|>deepcopy, iterloss, Θ, Xv, _Xv, RegressionType, Val{:identity}())
      updateAfromGAₜ!(Θ,Xv, _Xv,1) #update to the final values of A
      λ⁺ = currentloss(Θ, Xv, _Xv, projectvolumefromA(Θ,Xv,_Xv))
    end

    if Ncleanpanel < 500_000 && PARAM[:testiterresults]
      Aexpanded = view(Θ.A', Θ.ts, :)
      LAexpanded = view(Θ.A', Θ.tsM1, :)
      need2demean = genabsμₖ.(Aexpanded, LAexpanded, Xv.ws, Xv.RLws)
      factored = Xv.M * need2demean

      Fcoefs::Vector{Symbol} = Fcoefficients0(zs.Zms)
      Fwcoefs::Vector{Symbol} = (s->Symbol(:w,s)).(Fcoefs)
      FRLwcoefs::Vector{Symbol} = (s->Symbol(:RLw,s)).(Fcoefs)
      FAcoefs::Vector{Symbol} = (s->Symbol(:A,s)).(Fcoefs)
      FLAcoefs::Vector{Symbol} = (s->Symbol(:LA,s)).(Fcoefs)
      Ffcoefs::Vector{Symbol} = (s->Symbol(:f,s)).(Fcoefs)

      dfdebug = hcat(
        DataFrame(ts=Θ.ts, tsM1=Θ.tsM1, v=Xv.v, vtilde=Xv.ṽ),
        DataFrame(Xv.ws, Fwcoefs),
        DataFrame(Xv.RLws, FRLwcoefs),
        DataFrame(Aexpanded, FAcoefs),
        DataFrame(LAexpanded, FLAcoefs),
        DataFrame(factored, Ffcoefs))

      dfdebug |> CSV.write(
        "$(PARAM[:testpath])\\$(PARAM[:crspdatalabel])_opt$(iterresultsname(λ⁺|>round|>Int)).csv")
    end

    return Int(round(min(λ⁺, typemax(Int)÷10))), results
  end=#

  @assert false #should never reach here due to the finalizer
end

function updateΨ!(Θ::AbstractVolumePartsMCMC{TM, TV, T}, ::Val{:randomeffects}) where {TM,TV,T}
  #@info "TM:$TM, TV:$TV, T:$T"
  updateA₀!(Θ)
end

#this will create the mcmc chain
function createmcmc(Θ::AbstractVolumePartsMCMC; stateupdater::Val = Val{PARAM[:mcmcstateupdater]}())
  @unpack cc, Ψ = Θ
  @unpack Nburn = cc

  Niter = Nburn + size(cc.value,1)

  for i ∈ 1:Niter
    (i==Nburn+1) && @assert(cc.idx==1)
    updateΨ!(Θ, stateupdater)
    (i<Niter) && updateandincrement!(cc, Ψ, verify=true)
  end
end


using ArcadeLearningEnvironment
using ArgParse
using Cambrian
using CartesianGeneticProgramming
using Dates
using IICGP
using Random

# function extension
import Cambrian.mutate
import Cambrian.evaluate

s = ArgParseSettings()
@add_arg_table! s begin
    "--cfg"
    help = "configuration script"
    default = "cfg/test_mono.yaml"
    "--game"
    help = "game rom name"
    default = "assault"
    "--seed"
    help = "random seed for evolution"
    arg_type = Int
    default = 0
    "--ind"
    help = "individual for evaluation"
    arg_type = String
    default = ""
end
args = parse_args(ARGS, s)
const game = args["game"]
const seed = args["seed"]
Random.seed!(seed)

main_cfg, cont_cfg, reducer, bootstrap = IICGP.monocgp_config(args["cfg"], game)

const max_frames = main_cfg["max_frames"]
const stickiness = main_cfg["stickiness"]
const grayscale = main_cfg["grayscale"]
const downscale = main_cfg["downscale"]
const logid = cont_cfg.id
const state_ref = get_state_ref(game, seed)

function play_atari(
    reducer::Reducer,
    controller::CGPInd,
    lck::ReentrantLock;
    rom=game,
    seed=seed,
    rom_state_ref=state_ref,
    max_frames=max_frames,
    grayscale=grayscale,
    downscale=downscale,
    stickiness=stickiness
)
    # Random.seed!(seed)
    mt = MersenneTwister(seed)
    game = Game(rom, seed, lck=lck, state_ref=rom_state_ref)
    reward = 0.0
    frames = 0
    prev_action = Int32(0)
    while ~game_over(game.ale)
        if rand(mt) > stickiness || frames == 0
            s = get_state(game, grayscale, downscale)
            output = IICGP.process(reducer, controller, s)
            action = game.actions[argmax(output)]
        else
            action = prev_action
        end
        reward += act(game.ale, action)
        frames += 1
        prev_action = action
        if frames > max_frames
            break
        end
    end
    close!(game)
    [reward]
end

if length(args["ind"]) > 0
    ind = CGPInd(cfg, read(args["ind"], String))
    ftn = fitness(ind, inps, outs)
    println("Fitness: ", ftn)
else
    mutate(ind::CGPInd) = goldman_mutate(cont_cfg, ind)
    lck = ReentrantLock()
    fit(controller::CGPInd) = play_atari(reducer, controller, lck)
    evaluate(e::CGPEvolution) = IICGP.fitness_evaluate(e, e.fitness)
    e = CartesianGeneticProgramming.CGPEvolution(cont_cfg, fit)
    init_backup(logid, args["cfg"])
    run!(e)
end


"""
    cross(fs::AbstractVector{<:GP})

Creates a multi-output GP from an `AbstractVector` of `GP`s.
"""
function cross(fs::AbstractVector{<:AbstractGP})
    consistency_checks(fs)
    return CompositeGP((cross, fs), first(fs).gpc)
end

function consistency_checks(fs)
    @assert length(fs) >= 1
    @assert all([f.gpc == first(fs).gpc for f in fs])
end
Zygote.@nograd consistency_checks

const cross_args{T<:AbstractVector{<:AbstractGP}} = Tuple{typeof(cross), T}

function mean_vector((_, fs)::cross_args, x::BlockData)
    blks = map((f, blk)->mean_vector(f, blk), fs, blocks(x))
    return Array(mortar(blks))
end

function cov((_, fs)::cross_args, x::BlockData)
    Cs = reshape(map((f, blk)->cov(f, (cross, fs), blk, x), fs, blocks(x)), :, 1)
    return Array(mortar(reshape(Cs, :, 1)))
end

function cov_diag((_, fs)::cross_args, x::BlockData)
    cs = map(cov_diag, fs, blocks(x))
    return Array(mortar(cs))
end

function cov((_, fs)::cross_args, x::BlockData, x′::BlockData)
    Cs = reshape(map((f, blk)->cov(f, (cross, fs), blk, x′), fs, blocks(x)), :, 1)
    return Array(mortar(reshape(Cs, :, 1)))
end

function cov_diag((_, fs)::cross_args, x::BlockData, x′::BlockData)
    cs = map(cov_diag, fs, blocks(x), blocks(x′))
    return Array(mortar(cs))
end

function cov((_, fs)::cross_args, f′::AbstractGP, x::BlockData, x′::AV)
    Cs = reshape(map((f, x)->cov(f, f′, x, x′), fs, blocks(x)), :, 1)
    return Array(mortar(Cs))
end
function cov(f::AbstractGP, (_, fs)::cross_args, x::AV, x′::BlockData)
    Cs = reshape(map((f′, x′)->cov(f, f′, x, x′), fs, blocks(x′)), 1, :)
    return Array(mortar(Cs))
end

function cov_diag(args::cross_args, f′::AbstractGP, x::BlockData, x′::AV)
    return diag(cov(args, f′, x, x′))
end
function cov_diag(f::AbstractGP, args::cross_args, x::AV, x′::BlockData)
    return diag(cov(f, args, x, x′))
end

#
# Util for multi-process versions of `rand`, `logpdf`, and `elbo`.
#

function finites_to_block(fs::AV{<:FiniteGP})
    return FiniteGP(
        cross(map(f->f.f, fs)),
        BlockData(map(f->f.x, fs)),
        make_block_noise(map(f->f.Σy, fs)),
    )
end

make_block_noise(Σys::Vector{<:Diagonal}) = Diagonal(Vector(mortar(diag.(Σys))))
make_block_noise(Σys::Vector) = block_diagonal(Σys)

function _get_indices(fs)
    sz = cumsum(map(length, fs))
    return [sz[n] - length(fs[n]) + 1:sz[n] for n in eachindex(fs)]
end
Zygote.@nograd _get_indices

#
# multi-process `rand`
#

function rand(rng::AbstractRNG, fs::AV{<:FiniteGP}, N::Int)
    Y = rand(rng, finites_to_block(fs), N)
    idx = _get_indices(fs)
    return map(n->Y[idx[n], :], eachindex(fs))
end
rand(rng::AbstractRNG, fs::AV{<:FiniteGP}) = vec.(rand(rng, fs, 1))
rand(fs::AV{<:FiniteGP}, N::Int) = rand(Random.GLOBAL_RNG, fs, N)
rand(fs::AV{<:FiniteGP}) = vec.(rand(Random.GLOBAL_RNG, fs))

#
# multi-process `logpdf`
#

function logpdf(fs::AV{<:FiniteGP}, ys::Vector{<:AV{<:Real}})
    return logpdf(finites_to_block(fs), vcat(ys...))
end
logpdf(fs::Vector{<:Observation}) = logpdf(get_f.(fs), get_y.(fs))

#
# multi-process `elbo`
#

function elbo(fs::Vector{<:FiniteGP}, ys::Vector{<:AV{<:Real}}, us::Vector{<:FiniteGP})
    return elbo(finites_to_block(fs), Vector(mortar(ys)), finites_to_block(us))
end

function elbo(fs::Vector{<:FiniteGP}, ys::Vector{<:AV{<:Real}}, u::FiniteGP)
    return elbo(finites_to_block(fs), Vector(mortar(ys)), u)
end

elbo(f::FiniteGP, y::AV{<:Real}, us::Vector{<:FiniteGP}) = elbo(f, y, finites_to_block(us))


#
# Main function for Hybrid A* Trailer
#
# Author: Atsushi Sakai(@Atsushi_twi)
#

using PyPlot

include("../src/trailer_hybrid_a_star.jl")

function main()
    println(PROGRAM_FILE," start!!")

    # initial state
    sx = -15.0  # [m]
    sy = 8.0  # [m]
    syaw0 = deg2rad(180.0)
    syaw1 = deg2rad(180.0)

    # goal state
    gx = 1.0  # [m]
    gy = 3.0  # [m]
    gyaw0 = deg2rad(0.0)
    gyaw1 = deg2rad(0.0)

    # set obstacles
    ox = Float64[]
    oy = Float64[]

    for i in -25:25
        push!(ox, Float64(i))
        push!(oy, 15.0)
    end
    for i in -10:10
        push!(ox, Float64(i))
        push!(oy, 0.0)
    end
    for i in -25:-10
        push!(ox, Float64(i))
        push!(oy, 5.0)
    end
    for i in 10:25
        push!(ox, Float64(i))
        push!(oy, 5.0)
    end
    for i in 0:5
        push!(ox, 10.0)
        push!(oy, Float64(i))
    end
    for i in 0:5
        push!(ox, -10.0)
        push!(oy, Float64(i))
    end

    oox = ox[:]
    ooy = oy[:]
    # plot(oox, ooy, ".k")
    # axis("equal")
    # show()

    trailer_hybrid_a_star.trailerlib.plot_trailer.(gx, gy, gyaw0, gyaw1, 0)
    # path generation
    # @time path = trailer_hybrid_a_star.calc_hybrid_astar_path(sx, sy, syaw0, syaw1, gx, gy, gyaw0, gyaw1, ox, oy,
    #                                                            trailer_hybrid_a_star.XY_GRID_RESOLUTION,
    #                                                            trailer_hybrid_a_star.YAW_GRID_RESOLUTION
    #                                                            )

    # # ====Animation=====
    # show_animation(path, oox, ooy, sx, sy, syaw0, syaw1, gx, gy, gyaw0, gyaw1)

    println(PROGRAM_FILE," Done!!")
end

function show_animation(path, oox, ooy, sx, sy, syaw0, syaw1, gx, gy, gyaw0, gyaw1)
    plot(oox, ooy, ".k")
    trailer_hybrid_a_star.trailerlib.plot_trailer(sx, sy, syaw0, syaw1, 0.0)
    trailer_hybrid_a_star.trailerlib.plot_trailer(gx, gy, gyaw0, gyaw1, 0.0)
    x = path.x
    y = path.y
    yaw = path.yaw
    yaw1 = path.yaw1
    direction = path.direction

    steer = 0.0
    for ii in 1:length(x)
        cla()
        plot(oox, ooy, ".k")
        plot(x, y, "-r", label="Hybrid A* path")

        if ii < length(x)-1
            k = (yaw[ii+1] - yaw[ii])/trailer_hybrid_a_star.MOTION_RESOLUTION
            if !direction[ii]
                k *= -1
            end
            steer = atan(trailer_hybrid_a_star.WB*k, 1.0)
        else
            steer = 0.0
        end

        trailer_hybrid_a_star.trailerlib.plot_trailer.(x[ii], y[ii], yaw[ii], yaw1[ii], steer)
        grid(true)
        axis("equal")
        pause(0.0001)

    end

end

if length(PROGRAM_FILE)!=0 &&
	occursin(PROGRAM_FILE, @__FILE__)
    main()
end


module SPARK

    using Documenter: @doc
    using LinearAlgebra: Diagonal
    using RungeKutta
    using SimpleSolvers

    import GeometricEquations: LODE, LDAE, HDAE, IDAE, PDAE, get_functions

    using ..GeometricBase
    using ..Config

    import ..Solutions: AtomicSolutionPDAE, SolutionVector, update!

    import ..Integrators

    import ..Integrators: PDAEIntegrator, InitialGuessIODE, InitialGuessPODE, Parameters
    import ..Integrators: IDAEIntegratorCache, IntegratorCache, CacheDict, CacheType
    import ..Integrators: AbstractTableau, AbstractCoefficients,
                          @CoefficientsRK, @HeaderCoefficientsRK
    import ..Integrators: create_internal_stage_vector, create_nonlinear_solver,
                          update_vector_fields!, update_solution!, update_multiplier!,
                          initialize!
    import ..Integrators: equation, equations, tableau, timestep,
                          eachdim, eachstage, nstages

    import ..Utils: @define

    export CoefficientsARK, CoefficientsPRK, CoefficientsMRK, CoefficientsIRK,
           CoefficientsSPARK

    export AbstractIntegratorSPARK
    export AbstractTableauSPARK, TableauSPARK

    export IntegratorSPARK, TableauSPARK

    export IntegratorHPARK, TableauHPARK
    export IntegratorVPARK, TableauVPARK

    export IntegratorHSPARK, TableauHSPARK
    export IntegratorHSPARKprimary, TableauHSPARKprimary
    export IntegratorHSPARKsecondary, TableauHSPARKsecondary

    export IntegratorVSPARK, TableauVSPARK
    export IntegratorVSPARKprimary, TableauVSPARKprimary
    export IntegratorVSPARKsecondary, TableauVSPARKsecondary

    export IntegratorSLRK, TableauSLRK

    include("spark/abstract_integrator_spark.jl")
    include("spark/coefficients.jl")

    include("spark/integrators_spark_cache.jl")
    include("spark/integrators_spark_common.jl")
    include("spark/integrators_spark_tableau.jl")
    include("spark/integrators_spark_parameters.jl")

    include("spark/integrators_spark.jl")

    include("spark/integrators_vpark.jl")
    include("spark/integrators_hpark.jl")

    include("spark/integrators_vspark_common.jl")
    include("spark/integrators_vspark.jl")
    include("spark/integrators_vspark_primary.jl")
    include("spark/integrators_vspark_secondary.jl")

    include("spark/integrators_hspark_common.jl")
    include("spark/integrators_hspark.jl")
    include("spark/integrators_hspark_primary.jl")
    include("spark/integrators_hspark_secondary.jl")

    include("spark/integrators_slrk.jl")

end


function missing_cards(collection, current_deck)
    for elem in current_deck
        if elem.amount > sum_over_prints(collection, elem.prints)
            return true
        end
    end
    return false
end

function deck_status(; collection, current_deck)
    ans = NamedTuple{(:card, :total, :needed, :rarity),Tuple{Int64,Float64,Float64,Int64}}[]
    for elem in current_deck
        needed = max(0,elem.amount - sum_over_prints(collection, elem.prints))
        for print in elem.prints
            push!(ans, (card = print.index, total = elem.amount / length(elem.prints),
             needed = needed / length(elem.prints), rarity = print.rarity))
        end
    end
    ans
end

function can_finish_deck(collection, account, current_deck)
    needed_wcs = [0,0,0,0]
    for elem in current_deck
        needed = elem.amount - sum_over_prints(collection, elem.prints)
        if needed > 0
            # NOTE: Minimum rarity might not be best if rare WCs are bottleneck and
            # there are reprints between rare and mythic
            rarity = minimum(x -> x.rarity, elem.prints)
            needed_wcs[rarity] += needed
        end
    end
    all(needed_wcs[i] <= account.wc_count[i] for i in 1:4)
end

function spend_wildcards!(; account, collection, current_deck, output, rep,
     deckindex, logging_vector, parameters, openable_cards)
    for elem in current_deck
        r = minimum(x -> x.rarity, elem.prints)
        i = elem.prints[1].index
        set = elem.prints[1].set
        while (account.wc_count[r] > 0) && (elem.amount - sum_over_prints(collection, elem.prints) > 0)
            account.wc_count[r] -= 1
            output.wcs_spent_on_decks[rep,deckindex,r] += 1
            collection[i] += 1
            if collection[i] >= 4 && parameters.prevent_duplicates
                remove_fully_collected!(i, set, r; openable_cards = openable_cards)
            end
            push!(logging_vector, i)
        end
    end
end

function next_set!(collection, account, current_deck, sets, openable_cards)::Symbol
    for set in sets
        if account.bonus_packs[set] > 0
            return set
        end
    end

    count_sets_with_high_rarity!(set_counter, collection, current_deck, openable_cards )
    argmax(set_counter)
end

function next_set_duplicates!(collection, account, current_deck, sets, openable_cards)::Symbol
    for set in sets
        if account.bonus_packs[set] > 0
            return set
        end
    end

    best = openable_cards.sets[1]
    bestpct = 0.0
    for set in openable_cards.sets
        count = 0
        total = length(openable_cards.by_set_rarity[(set,3)])

        for i in openable_cards.by_set_rarity[(set,3)]
            if collection[i] >= 4
                count += 1
            end
        end

        if count / total > bestpct
            best = set
            bestpct = count / total
        end
    end

    count_sets_with_high_rarity!(set_counter, collection, current_deck, openable_cards )
    if bestpct > 0.25
        best
    else
        argmax(set_counter)
    end
end

function next_set_kld!(collection, account, current_deck, sets, openable_cards)::Symbol
    if in(:KLD, openable_cards.sets)
        :KLD
    else
        last(openable_cards.sets)
    end
end

function next_set_welcome_bundle!(collection, account, current_deck, sets, openable_cards)::Symbol
    count_sets_with_high_rarity!(set_counter, collection, current_deck, openable_cards )
    argmax(set_counter)
end

function next_set_m19!(collection, account, current_deck, sets, openable_cards)::Symbol
    :M19
end

function next_set_dom!(collection, account, current_deck, sets, openable_cards)::Symbol
    :DOM
end

function next_set_xln!(collection, account, current_deck, sets, openable_cards)::Symbol
    :XLN
end

function next_set_grn!(collection, account, current_deck, sets, openable_cards)::Symbol
    :GRN
end

function count_sets_with_high_rarity!(d::Dict{Symbol,Float64}, collection, deck, openable_cards, rarity = 3)
    for k in keys(d)
        d[k] = 0.0
    end

    for elem in deck
        for print in elem.prints
            if print.rarity == rarity && in(print.set, openable_cards.sets)
                d[print.set] += max(0.0, elem.amount - sum_over_prints(collection, elem.prints)) /
                   (length(openable_cards.by_set_rarity[(print.set, rarity)]))
            end
        end
    end

    d
end


module ThermodynamicsBase

include("abstracts.jl")
include("constants.jl")
include("statestructs.jl")
include("eos_api.jl")

export AbstractPhaseState, AbstractSubstance, AbstractThermoBackend,
AbstractMixture, AbstractEquationOfState, AbstractMixingRules, AbstractProperty

export R, thermocalc, thermocalc!

end


function project_position(scene, point, model)
    p4d = to_ndim(Vec4f0, to_ndim(Vec3f0, point, 0f0), 1f0)
    clip = scene.camera.projectionview[] * model * p4d
    p = (clip / clip[4])[Vec(1, 2)]
    p = collect((p .+ 1) ./ 2)
    w, h = scene.camera.resolution[]
    if w > h
        p[2:2:end] .*= (h / w)
    else
        p[1:2:end] .*= (w / h)
    end
    return Vec2f0(p)
end

project_scale(scene::Scene, s::Number) = project_scale(scene, Vec2f0(s))

function project_scale(scene::Scene, s)
    p4d = to_ndim(Vec4f0, s, 0f0)
    p = (scene.camera.projectionview[] * p4d)[Vec(1, 2)] ./ 2f0
    return Vec2f0(p)
end

const marker_translation_table = Dict(
    '■' => GR.MARKERTYPE_SOLID_SQUARE,
    '★' => GR.MARKERTYPE_SOLID_STAR,
    '◆' => GR.MARKERTYPE_SOLID_DIAMOND,
    '⬢' => GR.MARKERTYPE_HEXAGON,
    '✚' => GR.MARKERTYPE_SOLID_PLUS,
    '❌' => GR.MARKERTYPE_DIAGONAL_CROSS,
    '▲' => GR.MARKERTYPE_SOLID_TRI_UP,
    '▼' => GR.MARKERTYPE_SOLID_TRI_DOWN,
    '◀' => GR.MARKERTYPE_SOLID_TRI_LEFT,
    '▶' => GR.MARKERTYPE_SOLID_TRI_RIGHT,
    '⬟' => GR.MARKERTYPE_PENTAGON,
    '⯄' => GR.MARKERTYPE_OCTAGON,
    '✦' => GR.MARKERTYPE_STAR_4,
    '🟋' => GR.MARKERTYPE_STAR_6,
    '✷' => GR.MARKERTYPE_STAR_8,
    '┃' => GR.MARKERTYPE_VLINE,
    '━' => GR.MARKERTYPE_HLINE,
    '+' => GR.MARKERTYPE_PLUS,
    'x' => GR.MARKERTYPE_DIAGONAL_CROSS,
    '●' => GR.MARKERTYPE_SOLID_CIRCLE
)

# Color utilities
gr_color(c) = gr_color(c, color_type(c))
gr_color(c, ::Type) = gr_color(RGBA(c), RGB) # generic fallback
gr_color(c, ::Symbol) = gr_color(Colors.colorant(c), RGB)
function gr_color(c, ::Type{<:AbstractRGB})
    return UInt32(
        round(UInt, clamp(alpha(c) * 255, 0, 255)) << 24 +
        round(UInt,  clamp(blue(c) * 255, 0, 255)) << 16 +
        round(UInt, clamp(green(c) * 255, 0, 255)) << 8  +
        round(UInt,   clamp(red(c) * 255, 0, 255))
    )
end
function gr_color(c, ::Type{<:AbstractGray})
    g = round(UInt, clamp(gray(c) * 255, 0, 255))
    α = round(UInt, clamp(alpha(c) * 255, 0, 255))
    return UInt32( α<<24 + g<<16 + g<<8 + g )
end

function gr_colorind(c)
    convert(Int, GR.inqcolorfromrgb(red(c), green(c), blue(c)))
end

# Heatmap positioning
function is_uniformly_spaced(v; tol=1e-6)
  dv = diff(v)
  maximum(dv) - minimum(dv) < tol * mean(abs.(dv))
end

is_uniformly_spaced(::AbstractPlotting.ClosedInterval) = true

is_uniformly_spaced(::AbstractRange) = true

# surface utilities
# This function will return a vector form of `x` and `y`
# if needed, taking the lengths from `z`.
function ensure_vector(x::AbstractRange; size)
    return collect(x)
end
function ensure_vector(x::Vector; size)
    return x
end
function ensure_vector(x::ClosedInterval; size)
    return collect(LinRange(extrema(x)..., size))
end
# Backend utilities
struct GRBackend <: AbstractPlotting.AbstractBackend
end
struct GRScreen <: AbstractPlotting.AbstractScreen
    scene::Scene
end

function AbstractPlotting.backend_display(::GRBackend, scene::Scene)
    withenv("GKS_WSTYPE" => "0") do
        AbstractPlotting.update!(scene)
        GR.clearws()
        gr_draw(scene)
        GR.updatews()
    end
end

# TODO we need to revamp these!
Base.insert!(screen::GRScreen, scene::Scene, plot) = nothing

AbstractPlotting.push_screen!(scene::Scene, ::Nothing) = nothing

# Provides an in-memory buffer for the plot as an image.
# Used in the AbstractPlotting standard record function,
# but in the case of GR, that can be replaced by the GR
# native recording functions.
function AbstractPlotting.colorbuffer(scr::GRScreen)
    scene = scr.scene
    height, width = widths(pixelarea(scene)[])
    rcmat = Array{UInt8}(undef, 4, width, height) # GR outputs row major images to memory.
    ncmat = Array{RGBA}(undef, width, height)     # We need to return a matrix of colors.
    pstr = @sprintf "%p" Int(pointer(rcmat))      # Print in octal notation
    GR.beginprint("!$(width)x$(height)@$(pstr).mem")
    draw(scene)
    GR.endprint()
    for i in 1:height, j in 1:width
        ncmat[j, i] = AbstractPlotting.Colors.RGBA((rcmat[:, j, i] ./ 255)...) # divide by 255 to move to N0f8
    end

    return ncmat
end


function evaluator_tests(make_eval::Function)
    @testset "Basic evaluation with a single-objective function to be minimized" begin
        e = make_eval()
        @test BlackBoxOptim.num_evals(e) == 0

        fit1 = fitness([0.0, 1.0], e)
        @test BlackBoxOptim.num_evals(e) == 1
        @test BlackBoxOptim.last_fitness(e) == fit1

        fit2 = fitness([2.0, 1.0], e)
        @test BlackBoxOptim.num_evals(e) == 2
        @test BlackBoxOptim.last_fitness(e) == fit2

        @test BlackBoxOptim.numdims(e) == 2

        @test BlackBoxOptim.is_better(0.0, 1.0, e)
        @test BlackBoxOptim.is_better(0.0, -1.0, e) == false

        a, b = [0.0, 0.5], [1.0, 1.0]

        @test BlackBoxOptim.is_better(a, 1.0, e)
        @test BlackBoxOptim.last_fitness(e) == 0.25

        @test BlackBoxOptim.is_better(b, 1.0, e) == false
        @test BlackBoxOptim.last_fitness(e) == 2.0

        @test BlackBoxOptim.best_of(a, b, e) == (a, 0.25)
        @test BlackBoxOptim.best_of(b, a, e) == (a, 0.25)
        BlackBoxOptim.shutdown!(e)
    end

    @testset "update_fitness!()" begin
        e = make_eval()

        candidates = [random_candidate(2, -1.0, 1.0) for i in 1:10]
        BlackBoxOptim.update_fitness!(e, candidates)
        @test BlackBoxOptim.num_evals(e) == length(candidates)
        @test all(c -> isfinite(c.fitness), candidates)
        BlackBoxOptim.shutdown!(e)
    end

    @testset "rank_by_fitness!()" begin
        e = make_eval()

        candidates = [random_candidate(2, -1.0, 1.0) for i in 1:10]
        # partially evaluate fitness
        BlackBoxOptim.update_fitness!(e, candidates[1:5])
        @test BlackBoxOptim.num_evals(e) == 5
        # complete fitness evaluation and sort by it
        BlackBoxOptim.rank_by_fitness!(e, candidates)
        @test BlackBoxOptim.num_evals(e) == 10
        @test sortperm(candidates, by = fitness) == collect(1:10)
        BlackBoxOptim.shutdown!(e)
    end
end

function multiobj_evaluator_tests(make_eval::Function)
    @testset "multi-objective problem" begin
        schaffer1(x) = (sum(abs2, x), sum(xx -> abs2(xx - 2.0), x))
        p = BlackBoxOptim.FunctionBasedProblem(schaffer1, "Schaffer1", ParetoFitnessScheme{2}(is_minimizing=true),
                                               RectSearchSpace(5, (-10.0, 10.0)))
        a = EpsBoxArchive(EpsBoxDominanceFitnessScheme(fitness_scheme(p)), max_size=100)

        e = make_eval(p, a)
        fit1 = fitness([0.0, 1.0, 2.0, 3.0, 4.0], e)
        @test BlackBoxOptim.num_evals(e) == 1
        @test BlackBoxOptim.last_fitness(e) == fit1.orig

        fit2 = fitness([0.0, -1.0, -2.0, -3.0, -4.0], e)
        @test BlackBoxOptim.num_evals(e) == 2
        @test BlackBoxOptim.last_fitness(e) == fit2.orig

        BlackBoxOptim.shutdown!(e)
    end
end

@testset "Evaluator" begin
    # Set up a small example problem
    f(x) = sum(abs2, x)
    p = minimization_problem(f, "", (-1.0, 1.0), 2)
    @testset "ProblemEvaluator" begin
        evaluator_tests(() -> BlackBoxOptim.ProblemEvaluator(p))
    end

    @testset "rank_by_fitness!()" begin
        e = BlackBoxOptim.ProblemEvaluator(p)

        candidates = [random_candidate(2, -1.0, 1.0) for i in 1:10]
        # partially evaluate fitness
        BlackBoxOptim.update_fitness!(e, candidates[1:5])
        @test BlackBoxOptim.num_evals(e) == 5
        # complete fitness evaluation and sort by it
        BlackBoxOptim.rank_by_fitness!(e, candidates)
        @test BlackBoxOptim.num_evals(e) == 10
        @test sortperm(candidates, by = fitness) == collect(1:10)
    end

    @testset "ParallelEvaluator" begin
        using Distributed

        evaluator_tests(() -> BlackBoxOptim.ParallelEvaluator(p, pids=workers()))
        multiobj_evaluator_tests((p, a) -> BlackBoxOptim.ParallelEvaluator(p, a))
    end

    @testset "MultithreadEvaluator" begin
        if Threads.nthreads() > 1
            evaluator_tests(() -> BlackBoxOptim.MultithreadEvaluator(p))
            multiobj_evaluator_tests((p, a) -> BlackBoxOptim.MultithreadEvaluator(p, a))
        else
            @warn "MultithreadEvaluator tests require >1 threads, $(Threads.nthreads()) found, use JULIA_NUM_THREADS"
            @test_skip Threads.nthreads() > 1
        end
    end
end


module BCs

using DocStringExtensions
using Adapt

export LwBCs, SwBCs

"""
    LwBCs{FT,FTA1D,FTA2DN}

Longwave boundary conditions

# Fields
$(DocStringExtensions.FIELDS)
"""
struct LwBCs{FT, FTA2D, FTA2DN}
    "Surface emissivity `[W/m²]` `(nbnd, ncol)`"
    sfc_emis::FTA2D
    "incident flux at top of atmosphere `[W/m²]` `(ncol, ngpt)`"
    inc_flux::FTA2DN
end
LwBCs(sfc_emis, inc_flux) = LwBCs{eltype(sfc_emis), typeof(sfc_emis), typeof(inc_flux)}(sfc_emis, inc_flux)
Adapt.@adapt_structure LwBCs

"""
    SwBCs{FT,FTA1D}
Shortwave boundary conditions
# Fields
$(DocStringExtensions.FIELDS)
"""
struct SwBCs{FT, FTA1D, FTA1DN, FTA2D}
    "zenith angle `(ncol)`"
    zenith::FTA1D
    "top of atmosphere flux `(ncol)`"
    toa_flux::FTA1D
    "surface albedo for specular (direct) radiation `(nbnd, ncol)`"
    sfc_alb_direct::FTA2D
    "incident diffuse flux at top of domain `[W/m2]` `(ncol, ngpt)`"
    inc_flux_diffuse::FTA1DN
    "surface albedo for diffuse radiation `(nbnd, ncol)`"
    sfc_alb_diffuse::FTA2D
end
SwBCs(zenith, toa_flux, sfc_alb_direct, inc_flux_diffuse, sfc_alb_diffuse) =
    SwBCs{eltype(zenith), typeof(zenith), typeof(inc_flux_diffuse), typeof(sfc_alb_direct)}(
        zenith,
        toa_flux,
        sfc_alb_direct,
        inc_flux_diffuse,
        sfc_alb_diffuse,
    )
Adapt.@adapt_structure SwBCs

end


include("abstractfeatures.jl")

"""
    PairFeatureDescriptor{A}

A feature object that encodes features associated with pairs of atoms in a structure that may or may not be bonded to each other. For bond features, see `BondFeatureDescriptor`.

Type parameter represents the structure representation(s) from which this feature descriptor is able to compute features.

## Fields
- `name::String`: the name of the feature
- `compute_f`: function that takes in a structure of type `A` and returns values of this feature for every pair of atoms in the structure organized in a matrix
- `encoder_decoder::AbstractCodec`: codec that encodes/decodes values of this feature
- `categorical::Bool`: flag for whether the feature is categorical or continuous-valued
- `encodable_elements::Vector{String}`: list of elements (by symbol) that can be encoded by this feature
"""
struct PairFeatureDescriptor{A,C<:AbstractCodec} <: AbstractPairFeatureDescriptor
    name::String
    compute_f::Function
    encoder_decoder::C
    categorical::Bool
    encodable_elements::Vector{String}
end

# pretty printing, long version
function Base.show(io::IO, ::MIME"text/plain", fd::PairFeatureDescriptor{A}) where {A}
    st = "$(typeof(fd)) $(fd.name):\n   categorical: $(fd.categorical)\n   works on: $(A)"
    print(io, st)
end

encodable_elements(fd::PairFeatureDescriptor) = fd.encodable_elements

function get_value(pfd::PairFeatureDescriptor{A}, a::AbstractAtoms{<:A}) where {A}
    @assert all([el in encodable_elements(pfd) for el in elements(a)]) "Feature $(pfd.name) cannot encode some element(s) in this structure!"
    pfd.compute_f(a.structure)
end


# MathOptInterface NLP

MOI.supports(::Optimizer, ::MOI.NLPBlock) = true

function MOI.set(model::Optimizer, ::MOI.NLPBlock, nlp_data::MOI.NLPBlockData)
    return model.nlp_data = nlp_data
end

# Keep loading of NLP constraints apart to load all NLP model all in once
# inside Knitro.
function load_nlp_constraints(model::Optimizer)
    num_nlp_constraints = length(model.nlp_data.constraint_bounds)

    # We need to load the NLP constraints inside Knitro.
    if num_nlp_constraints > 0
        num_cons = KN_add_cons(model.inner, num_nlp_constraints)

        for (ib, pair) in enumerate(model.nlp_data.constraint_bounds)
            if pair.upper == pair.lower
                KN_set_con_eqbnd(model.inner, num_cons[ib], pair.upper)
            else
                ub = check_value(pair.upper)
                lb = check_value(pair.lower)
                KN_set_con_upbnd(model.inner, num_cons[ib], ub)
                KN_set_con_lobnd(model.inner, num_cons[ib], lb)
            end
        end
        # Add constraint to index.
        model.nlp_index_cons = num_cons
    end
    return
end

function MOI.supports(::Optimizer, ::MOI.NLPBlockDualStart)
    return true
end

# TODO: FIXME
function MOI.set(model::Optimizer, ::MOI.NLPBlockDualStart, values)
    # @assert length(values) == length(model.nlp_index_cons)
    KN_set_con_dual_init_values(model.inner, Cint[0], values)
    return
end


# core mechanism of extending Taine Zhao's @thautwarm 's MatchCore pattern matching.

using MatchCore

## compile (quote) left and right hands of a rule
# escape symbols to create MLStyle compatible patterns

# compile left hand of rule
# if it's the first time seeing v, add it to the "seen symbols" Set
# insert a :& expression only if v has not been seen before
function c_left(v::Symbol, s)
    if Base.isbinaryoperator(v) return v end
    (v ∉ s ? (push!(s, v); v) : amp(v)) |> dollar
end
c_left(v::Expr, s) = v.head ∈ add_dollar ? dollar(v) : v
# support symbol literals in left hand
c_left(v::QuoteNode, s) = v.value isa Symbol ? dollar(v) : v
c_left(v, s) = v # ignore other types

c_right(v::Symbol) = Base.isbinaryoperator(v) ? v : dollar(v)
function c_right(v::Expr)
    v.head ∈ add_dollar ? dollar(v) : v
end
c_right(v::QuoteNode) = v.value isa Symbol ? v.value : v
c_right(v) = v #ignore other types

# add dollar in front of the expressions with those symbols as head
const add_dollar = [:(::), :(...)]
# don't walk down on these symbols
const skips = [:(::), :(...)]

# Compile rules from Metatheory format to MatchCore format
function compile_rule(rule::Rule)::Expr
    le = df_walk(c_left, rule.left, Vector{Symbol}(); skip=skips, skip_call=true) |> quot

    if rule.mode == :dynamic # regular pattern matching
        # right side not quoted! needed to evaluate expressions in right hand.
        re = rule.right
    elseif rule.mode == :rewrite || rule.mode == :equational
		# right side is quoted, symbolic replacement
        re = df_walk(c_right, rule.right; skip=skips, skip_call=true) |> quot
	else
        error(`rule "$e" is not in valid form.\n`)
    end

    return :($le => $re)
end

# catch-all for reductions
const identity_axiom = :($(quot(dollar(:i))) => i)

# TODO analyse theory before compiling. Identify associativity and commutativity
# and other loop-creating problems. Generate a pattern matching block with the
# correct rule order and expansions for associativity and distributivity
# import Iterators: flatten

function theory_block(t::Vector{Rule})
	tn = Vector{Expr}()

	for r ∈ t
		push!(tn, compile_rule(r))
		if r.mode == :equational
			mirrored = Rule(r.right, r.left, r.expr, r.mode, nothing)
			push!(tn, compile_rule(mirrored))
		end
	end

	block(tn..., identity_axiom)
end

"""
Compile a theory to a closure that does the pattern matching job
Returns a RuntimeGeneratedFunction, which does not use eval and
is as fast as a regular Julia anonymous function 🔥
"""
function compile_theory(theory::Vector{Rule}, mod::Module; __source__=LineNumberNode(0))
    # generate an unique parameter name
    parameter = Meta.gensym(:reducing_expression)
    block = theory_block(theory)

    matching = MatchCore.gen_match(parameter, block, __source__, mod)
    matching = MatchCore.AbstractPatterns.init_cfg(matching)

    ex = :(($parameter) -> $matching)
    closure_generator(mod, ex)
end

"""
Compile a theory at runtime to a closure that does the pattern matching job
"""
macro compile_theory(theory)
    gettheory(theory, __module__)
end

# Retrieve a theory from a module at compile time. Not exported
function gettheory(var, mod; compile=true)
	t = nothing
    if Meta.isexpr(var, :block) # @matcher begine rules... end
		t = rmlines(macroexpand(mod, var)).args .|> Rule
	else
		t = mod.eval(var)
	end

	if compile && !(t isa Function)
		t = compile_theory(t, mod)
	end

	return t
end


# Wrapper for level-1 BLIS matrix routines.
#

# Level1v API of common forms are put together.
#
macro blis_group_level1m_form1(funcname)
    return quote
        @blis_ccall_group($funcname,
                          Cvoid,
                          diagoffa, BliDoff,
                          diaga,    BliDiag,
                          uploa,    BliUpLo,
                          transa,   BliTrans,
                          m,        BliDim,
                          n,        BliDim,
                          a,        Ptr{xType}, rsa, BliInc, csa, BliInc,
                          b,        Ptr{xType}, rsb, BliInc, csb, BliInc)
    end
end

@blis_group_level1m_form1 addm
@blis_group_level1m_form1 copym
@blis_group_level1m_form1 subm

macro blis_group_level1m_form2(funcname)
    return quote
        @blis_ccall_group($funcname,
                          Cvoid,
                          diagoffa, BliDoff,
                          diaga,    BliDiag,
                          uploa,    BliUpLo,
                          transa,   BliTrans,
                          m,        BliDim,
                          n,        BliDim,
                          α,        Ptr{xType},
                          a,        Ptr{xType}, rsa, BliInc, csa, BliInc,
                          b,        Ptr{xType}, rsb, BliInc, csb, BliInc)
    end
end

@blis_group_level1m_form2 axpym
@blis_group_level1m_form2 scal2m

macro blis_group_level1m_form3(funcname)
    return quote
        @blis_ccall_group($funcname,
                          Cvoid,
                          conjα,    BliConj,
                          diagoffa, BliDoff,
                          diaga,    BliDiag,
                          uploa,    BliUpLo,
                          m,        BliDim,
                          n,        BliDim,
                          α,        Ptr{xType},
                          a,        Ptr{xType}, rsa, BliInc, csa, BliInc)
    end
end

@blis_group_level1m_form3 scalm
@blis_group_level1m_form3 setm


# -------- Benchmarks --------
struct Benchmark
    label::String
    words::Int64 # bytes = words * sizeof(Float64) * N
    flops::Int64
    write_alloc_factor::Float64 # bytes = words * write_alloc_factor * sizeof(Float64) * N
end

const BENCHMARKS = [
    Benchmark("Init", 1, 0, 2.0),
    Benchmark("Copy", 2, 0, 3/2),
    Benchmark("Update", 2, 1, 1.0),
    Benchmark("Triad", 3, 2, 4/3),
    Benchmark("Daxpy", 3, 2, 1.0),
    Benchmark("STriad", 4, 2, 5/4),
    Benchmark("SDaxpy", 4, 2, 1.0)
]

const NBENCH = length(BENCHMARKS)

# -------- Kernel (first `nthreads` threads) --------
function init_kernel(a, scalar; thread_indices, nthreads)
    @threads :static for tid in 1:nthreads
        @inbounds for i in thread_indices[tid]
            a[i] = scalar
        end
    end
    return nothing
end

function copy_kernel(a, b; thread_indices, nthreads)
    @threads :static for tid in 1:nthreads
        @inbounds for i in thread_indices[tid]
            a[i] = b[i]
        end
    end
    return nothing
end

function update_kernel(a, scalar; thread_indices, nthreads)
    @threads :static for tid in 1:nthreads
        @inbounds for i in thread_indices[tid]
            a[i] = a[i] * scalar
        end
    end
    return nothing
end

function triad_kernel(a, b, c, scalar; thread_indices, nthreads)
    @threads :static for tid in 1:nthreads
        @inbounds for i in thread_indices[tid]
            a[i] = b[i] + scalar * c[i]
        end
    end
    return nothing
end

function daxpy_kernel(a, b, scalar; thread_indices, nthreads)
    @threads :static for tid in 1:nthreads
        @inbounds for i in thread_indices[tid]
            a[i] = a[i] + scalar * b[i]
        end
    end
    return nothing
end

function striad_kernel(a, b, c, d; thread_indices, nthreads)
    @threads :static for tid in 1:nthreads
        @inbounds for i in thread_indices[tid]
            a[i] = b[i] + d[i] * c[i]
        end
    end
    return nothing
end

function sdaxpy_kernel(a, b, c; thread_indices, nthreads)
    @threads :static for tid in 1:nthreads
        @inbounds for i in thread_indices[tid]
            a[i] = a[i] + b[i] * c[i]
        end
    end
    return nothing
end

# -------- Kernel (all threads) --------
function init_kernel_allthreads(a, scalar)
    @threads :static for i in eachindex(a)
        @inbounds a[i] = scalar
    end
    return nothing
end

function copy_kernel_allthreads(a, b)
    @threads :static for i in eachindex(a)
        @inbounds a[i] = b[i]
    end
    return nothing
end

function update_kernel_allthreads(a, scalar)
    @threads :static for i in eachindex(a)
        @inbounds a[i] = a[i] * scalar
    end
    return nothing
end

function triad_kernel_allthreads(a, b, c, scalar)
    @threads :static for i in eachindex(a)
        @inbounds a[i] = b[i] + scalar * c[i]
    end
    return nothing
end

function daxpy_kernel_allthreads(a, b, scalar)
    @threads :static for i in eachindex(a)
        @inbounds a[i] = a[i] + scalar * b[i]
    end
    return nothing
end

function striad_kernel_allthreads(a, b, c, d)
    @threads :static for i in eachindex(a)
        @inbounds a[i] = b[i] + d[i] * c[i]
    end
    return nothing
end

function sdaxpy_kernel_allthreads(a, b, c)
    @threads :static for i in eachindex(a)
        @inbounds a[i] = a[i] + b[i] * c[i]
    end
    return nothing
end