text:
mutable struct PARAMS
  algorithms::Array{String,1}
  training_data::Array{Float64,2}
  dist::String
  K_sigma::Float64
  KNN_k::Int64
  REC_varepsilon::Float64
  Q::Array{Float64,2} #covariance matrix
  mv::Array{Float64,2} # mean vector
  SVDD_nu::Float64
  SVDD_model
  KNFST_model
  # initialisation
  D::Tuple{Array{Float64,2},Array{Float64,2},Array{Float64,2}}
  K::Array{Float64,2}
  # algorithms
  KDE::Array{Float64,1}
  REC::Array{Float64,1}
  KNN #::Tuple{Int64,Array{Int64,1},Array{Float64,1},Array{Int64,2},Array{Float64,2}}
  KNN_Gamma::Array{Float64,1}
  KNN_Delta# ::Tuple{Array{Float64,1},Array{Float64,2},Array{Float64,2}}
  T2 #::Tuple{Array{Float64,1},Array{Float64,2},Array{Float64,2},Array{Float64,2},Array{Float64,2}}
  D_train::Tuple{Array{Float64,2},Array{Float64,2},Array{Float64,2}}
  D_test::Tuple{Array{Float64,2},Array{Float64,2},Array{Float64,2},Array{Float64,2},Array{Float64,2}}
  KNFST
  SVDD # ::Tuple{Array{Any,1},Array{Float64,2},Array{LIBSVM.SVMNode,2},Array{Ptr{LIBSVM.SVMNode},1}}
  REC_quantiles::Array{Float64,1}
  KDE_quantiles::Array{Float64,1}
  KNN_Gamma_quantiles::Array{Float64,1}
  KNN_Delta_quantiles::Array{Float64,1}
  T2_quantiles::Array{Float64,1}
  KNFST_quantiles::Array{Float64,1}
  SVDD_quantiles::Array{Float64,1}
  data::Array{Float64,2}
  temp_excl::Int64
  ensemble_method::String
  ensemble
  quantiles::Bool
end

function reshape_dims_except_N(datacube::AbstractArray{tp,N}) where {tp, N}
  dims = 1
    for i = 1:(N-1) # multiply dimensions except the last one and save as dims
        dims = size(datacube, i) * dims
    end
    X = reshape(datacube, dims, size(datacube, N))
    return(X)
end

function ispartof(needle::Array{String,1}, haystack::Array{String,1})
  partof = fill(false, size(haystack, 1))
  for i = 1:size(needle, 1)
    partof[needle[i] .== haystack] .= true
  end
  return(partof)
end

"""
    getParameters(algorithms::Array{String,1} = ["REC", "KDE"], training_data::AbstractArray{tp, 2} = [NaN NaN])

return an object of type PARAMS, given the `algorithms` and some `training_data` as a matrix.

# Arguments
- `algorithms`: Subset of `["REC", "KDE", "KNN_Gamma", "KNN_Delta", "SVDD", "KNFST", "T2"]`
- `training_data`: data for training the algorithms / for getting the Parameters.
- `dist::String = "Euclidean"`
- `sigma_quantile::Float64 = 0.5` (median): quantile of the distance matrix, used to compute the weighting parameter for the kernel matrix (`algorithms = ["SVDD", "KNFST", "KDE"]`)
- `varepsilon_quantile` = `sigma_quantile` by default: quantile of the distance matrix to compute the radius of the hyperball in which the number of reccurences is counted (`algorihtms = ["REC"]`)
- `k_perc::Float64 = 0.05`: percentage of the first dimension of `training_data` to estimmate the number of nearest neighbors (`algorithms = ["KNN-Gamma", "KNN_Delta"]`)
- `nu::Float64 = 0.2`: use the maximal percentage of outliers for `algorithms = ["SVDD"]`
- `temp_excl::Int64 = 0`. Exclude temporal adjacent points from beeing count as recurrences of k-nearest neighbors `algorithms = ["REC", "KNN-Gamma", "KNN_Delta"]`
- `ensemble_method = "None"`: compute an ensemble of the used algorithms. Possible choices (given in `compute_ensemble()`) are "mean", "median", "max" and "min".
- `quantiles = false`: convert the output scores of the algorithms into quantiles.

# Examples
```
julia> using MultivariateAnomalies
julia> training_data = randn(100, 2); testing_data = randn(100, 2);
julia> P = getParameters(["REC", "KDE", "SVDD"], training_data, quantiles = false);
julia> detectAnomalies(testing_data, P)
```
"""
function getParameters(algorithms::Array{String,1} = ["REC", "KDE"], training_data::AbstractArray{tp, N} = [NaN NaN]; dist::String = "Euclidean", sigma_quantile::Float64 = 0.5, varepsilon_quantile::Float64 = NaN, k_perc::Float64 = 0.05, nu::Float64 = 0.2, temp_excl::Int64 = 0, ensemble_method = "None", quantiles = false) where {tp, N}

  allalgorithms = ["KDE", "REC", "KNN_Delta", "KNN_Gamma", "T2", "SVDD", "KNFST"]
  @assert any(ispartof(algorithms, allalgorithms))
  if(length(algorithms) != sum(ispartof(algorithms, allalgorithms)))
    error("one or more of algorithms $algorithms are not within the defined possibilites $allalgorithms")
  end

  T = size(training_data, 1)

  if(length(size(training_data)) > 2) training_data = reshape_dims_except_N(training_data) end

  #Parameters = PARAMS(algorithms, training_data, dist, NaN, 0, NaN, [NaN NaN], [NaN NaN], NaN, NaN, NaN)
    P = PARAMS(algorithms, training_data, "Euclidean"
               , NaN, 0, NaN, [NaN NaN], [NaN NaN], nu, NaN, NaN
                      , ([NaN NaN], [NaN NaN], [NaN NaN]) #D
                      , [NaN NaN] # K
                      ,  [NaN] # KDE
                      , [NaN] # REC
                      , NaN #KNN
                      , [NaN] # KNN_Gamma
                      , NaN # KNN_Delta
                      , NaN # T2
                      , ([NaN NaN], [NaN NaN], [NaN NaN]) # D_train
                      , ([NaN NaN], [NaN NaN], [NaN NaN], [NaN NaN], [NaN NaN])  # D test
                      , NaN # KNFST
                      , NaN # SVDD
                              , [NaN], [NaN], [NaN], [NaN], [NaN], [NaN], [NaN] # quantiles
                      , [NaN NaN]# data
                      , temp_excl, ensemble_method, NaN, quantiles
                         )
  if(any(dist .== ["Mahalanobis", "SqMahalanobis"]) || any(algorithms .== "T2"))
      P.Q = cov(training_data)
      P.mv = mean(training_data, dims = 1)
  end
  if(any(algorithms .== "REC") || any(algorithms .== "KDE") || any(algorithms .== "SVDD") || any(algorithms .== "KNFST"))
    D_train = dist_matrix(training_data, dist = P.dist, Q = P.Q)
    if(isnan(varepsilon_quantile)) varepsilon_quantile = sigma_quantile end
    (P.K_sigma, P.REC_varepsilon) = quantile(unsafe_wrap(Array,pointer(D_train), length(D_train)), [sigma_quantile, varepsilon_quantile])
  end
  if(any(algorithms .== "KNN_Gamma") || any(algorithms .== "KNN_Delta"))
    P.KNN_k = Int(ceil(k_perc * T))
  end
  if(any(algorithms .== "SVDD") || any(algorithms .== "KNFST"))
    kernel_matrix!(D_train, D_train, P.K_sigma)
    if(any(algorithms .== "KNFST"))  P.KNFST_model = KNFST_train(D_train) end
    if(any(algorithms .== "SVDD"))   P.SVDD_model = SVDD_train(D_train, nu) end
  end

  return(P)
end

"""
    init_detectAnomalies{tp, N}(data::AbstractArray{tp, N}, P::PARAMS)

initialize empty arrays in `P` for detecting the anomalies.
"""
function init_detectAnomalies(data::AbstractArray{tp, N}, P::PARAMS) where {tp, N}
# initialisation
  T = size(data, 1)
  VARs = size(data, N)
  P.data = zeros(tp, T, VARs)
  P.D = init_dist_matrix(data)
  P.K = similar(P.D[1])
  if(any(P.algorithms .== "KDE")) P.KDE = init_KDE(T) end
  if(any(P.algorithms .== "REC")) P.REC = init_REC(T) end

  if(any(P.algorithms .== "KNN_Gamma") || any(P.algorithms .== "KNN_Delta"))
    P.KNN = init_knn_dists(data, P.KNN_k)
    if(any(P.algorithms .== "KNN_Gamma")) P.KNN_Gamma = init_KNN_Gamma(T) end
    if(any(P.algorithms .== "KNN_Delta")) P.KNN_Delta = init_KNN_Delta(T, VARs, P.KNN_k) end
  end

  if(any(P.algorithms .== "T2")) P.T2 =  init_T2(data) end

  if(any(P.algorithms .== "SVDD") || any(P.algorithms .== "KNFST"))
    P.D_train = init_dist_matrix(P.training_data)
    P.D_test = init_dist_matrix(data, P.training_data)
    if(any(P.algorithms .== "KNFST")) P.KNFST = init_KNFST(T, P.KNFST_model) end
    #if(any(P.algorithms .== "SVDD"))  P.SVDD = init_SVDD_predict(T, size(P.training_data, 1)) end
  end

  return(P)
end

"""
    detectAnomalies!{tp, N}(data::AbstractArray{tp, N}, P::PARAMS)

mutating version of `detectAnomalies()`. Directly writes the output into `P`.
"""
function detectAnomalies!(data::AbstractArray{tp, 2}, P::PARAMS) where {tp}
  copyto!(P.data, data)
  allalgorithms = ["KDE", "REC", "KNN_Delta", "KNN_Gamma", "T2", "SVDD", "KNFST"]
  @assert any(ispartof(P.algorithms, allalgorithms))

  if(any(ispartof(P.algorithms, ["KDE", "REC", "KNN_Delta", "KNN_Gamma"])))   # D based
    dist_matrix!(P.D, P.data, dist = P.dist, Q = P.Q)
    if(any(ispartof(P.algorithms, ["KDE"])))
      kernel_matrix!(P.K, P.D[1], P.K_sigma)
      KDE!(P.KDE, P.K)
    end

     if(any(ispartof(P.algorithms, ["REC"])))
      REC!(P.REC, P.D[1], P.REC_varepsilon, P.temp_excl)
    end

    if(any(ispartof(P.algorithms, ["KNN_Gamma", "KNN_Delta"])))
      knn_dists!(P.KNN, P.D[1], P.temp_excl)

      if(any(ispartof(P.algorithms, ["KNN_Gamma"]))) KNN_Gamma!(P.KNN_Gamma, P.KNN) end
      if(any(ispartof(P.algorithms, ["KNN_Delta"]))) KNN_Delta!(P.KNN_Delta, P.KNN, P.data) end

    end
  end

  if(any(ispartof(P.algorithms, ["T2"])))
    T2!(P.T2, P.data, P.Q, P.mv)
  end

  if(any(ispartof(P.algorithms, ["SVDD", "KNFST"])))
    dist_matrix!(P.D_train, P.training_data, dist = P.dist, Q = P.Q)
    dist_matrix!(P.D_test, P.data, P.training_data, dist = P.dist, Q = P.Q)
    kernel_matrix!(P.D_train[1],P.D_train[1], P.K_sigma) # transform distance to kernel matrix
    kernel_matrix!(P.D_test[1], P.D_test[1], P.K_sigma) # transform distance to kernel matrix
    if(any(ispartof(P.algorithms, ["KNFST"])))  KNFST_predict!(P.KNFST, P.KNFST_model, P.D_test[1]) end
    if(any(ispartof(P.algorithms, ["SVDD"])))
      (predicted_labels, decision_values) = SVDD_predict(P.SVDD_model, P.D_test[1])
      P.SVDD = broadcast(*, decision_values, -1)
    end
  end

  if(P.quantiles)
    if(any(ispartof(P.algorithms, ["T2"])))  P.T2_quantiles = get_quantile_scores(P.T2[1]) end
    if(any(ispartof(P.algorithms, ["REC"]))) P.REC_quantiles = get_quantile_scores(P.REC) end
    if(any(ispartof(P.algorithms, ["KDE"]))) P.KDE_quantiles = get_quantile_scores(P.KDE) end
    if(any(ispartof(P.algorithms, ["SVDD"]))) P.SVDD_quantiles = squeeze(get_quantile_scores(P.SVDD), 1) end
    if(any(ispartof(P.algorithms, ["KNFST"]))) P.KNFST_quantiles = get_quantile_scores(P.KNFST[1]) end
    if(any(ispartof(P.algorithms, ["KNN_Gamma"]))) P.KNN_Gamma_quantiles = get_quantile_scores(P.KNN_Gamma) end
    if(any(ispartof(P.algorithms, ["KNN_Delta"]))) P.KNN_Delta_quantiles = get_quantile_scores(P.KNN_Delta[1]) end
  end

  if(P.ensemble_method != "None")
    L = length(P.algorithms)
    if(L > 4 || L < 2) print("compute_ensemble() does currently only support 2-4 detection algorihms. You selected $L. \n") end
    if(!P.quantiles) print("Warning: P.quantiles should be true for computing ensembles out of comparable scores, but is false")
      @assert !any(ispartof(P.algorithms, ["KNN_Delta", "SVDD", "KNFST", "T2"]))
      if(L == 2) P.ensemble = compute_ensemble(getfield(P, Meta.parse(P.algorithms[1])), getfield(P, Meta.parse(P.algorithms[2])), ensemble = P.ensemble_method) end
      if(L == 3) P.ensemble = compute_ensemble(getfield(P, Meta.parse(P.algorithms[1])), getfield(P, Meta.parse(P.algorithms[2])), getfield(P, Meta.parse(P.algorithms[3])), ensemble = P.ensemble_method) end
      if(L == 4) P.ensemble = compute_ensemble(getfield(P, Meta.parse(P.algorithms[1])), getfield(P, Meta.parse(P.algorithms[2])), getfield(P, Meta.parse(P.algorithms[3])), getfield(P, Meta.parse(P.algorithms[4])), ensemble = P.ensemble_method) end
    else
      if(L == 2) P.ensemble = compute_ensemble(getfield(P, Meta.parse(string(P.algorithms[1], "_quantiles"))), getfield(P, Meta.parse(string(P.algorithms[2], "_quantiles"))), ensemble = P.ensemble_method) end
      if(L == 3) P.ensemble = compute_ensemble(getfield(P, Meta.parse(string(P.algorithms[1], "_quantiles"))), getfield(P, Meta.parse(string(P.algorithms[2], "_quantiles"))), getfield(P, Meta.parse(string(P.algorithms[3], "_quantiles"))), ensemble = P.ensemble_method) end
      if(L == 4) P.ensemble = compute_ensemble(getfield(P, Meta.parse(string(P.algorithms[1], "_quantiles"))), getfield(P, Meta.parse(string(P.algorithms[2], "_quantiles"))), getfield(P, Meta.parse(string(P.algorithms[3], "_quantiles"))), getfield(P, Meta.parse(string(P.algorithms[4], "_quantiles"))), ensemble = P.ensemble_method) end
    end
  end

  return(P)
end

"""
    detectAnomalies(data::AbstractArray{tp, N}, P::PARAMS) where {tp, N}
    detectAnomalies(data::AbstractArray{tp, N}, algorithms::Array{String,1} = ["REC", "KDE"]; mean = 0) where {tp, N}

detect anomalies, given some Parameter object `P` of type PARAMS. Train the Parameters `P` with `getParameters()` beforehand on some training data. See `getParameters()`.
Without training `P` beforehand, it is also possible to use `detectAnomalies(data, algorithms)` given some algorithms (except SVDD, KNFST).
Some default parameters are used in this case to initialize `P` internally.

# Examples
```
julia> training_data = randn(100, 2); testing_data = randn(100, 2);
julia> # compute the anoamly scores of the algorithms "REC", "KDE", "T2" and "KNN_Gamma", their quantiles and return their ensemble scores
julia> P = getParameters(["REC", "KDE", "T2", "KNN_Gamma"], training_data, quantiles = true, ensemble_method = "mean");
julia> detectAnomalies(testing_data, P)
```
"""
function detectAnomalies(data::AbstractArray{tp, 2}, P::PARAMS) where {tp}
  init_detectAnomalies(data, P)
  detectAnomalies!(data, P)
  return(return_detectAnomalies(P))
end

function return_scores(i, P::PARAMS)
  if(isa(getfield(P, Meta.parse(P.algorithms[i])), Tuple))
    return(getfield(P, Meta.parse(P.algorithms[i]))[1])
  else
    return(getfield(P, Meta.parse(P.algorithms[i])))
  end
end

function return_quantile_scores(i, P::PARAMS)
    return(getfield(P, Meta.parse(string(P.algorithms[i], "_quantiles"))))
end

function return_detectAnomalies(P::PARAMS)
  L = length(P.algorithms)
  if(any(ispartof([P.ensemble_method], ["mean","max","min", "median"]))) return(P.ensemble)
  elseif(L == 1 && !P.quantiles)  return(return_scores(1,P))
  elseif(!P.quantiles && L > 1) return(ntuple(i->return_scores(i,P), L))
  elseif(L > 1 && P.quantiles) return(ntuple(i->return_quantile_scores(i,P), L))
  elseif(L == 1 && P.quantiles) return(return_quantile_scores(1,P))
  end
end

function detectAnomalies(data::AbstractArray{tp, N}, algorithms::Array{String,1} = ["REC", "KDE"]; mean = 0, dist = "Euclidean") where {tp, N}
  @assert !any(ispartof(algorithms, ["SVDD", "KNFST"]))
   Q = cov(reshape_dims_except_N(data))
   if(mean == 0) meanvec = zeros(Float64, 1, size(data, N))
   else  meanvec = mean(data, dims = 1) end
   D = dist_matrix(data; dist = dist)
   sigma =  median(unsafe_wrap(Array, pointer(D), length(D)))
   P = PARAMS(algorithms, [NaN NaN], dist, sigma # sigma
             , Int(ceil(0.05 * size(data, 1))) # k
             , sigma # varepsilon
             , Q
             , meanvec, 0.2, NaN, NaN
                      , ([NaN NaN], [NaN NaN], [NaN NaN]) #D
                      , [NaN NaN] # K
                      ,  [NaN] # KDE
                      , [NaN] # REC
                      , NaN #KNN
                      , [NaN] # KNN_Gamma
                      , NaN # KNN_Delta
                      , NaN # T2
                      , ([NaN NaN], [NaN NaN], [NaN NaN]) # D_train
                      , ([NaN NaN], [NaN NaN], [NaN NaN], [NaN NaN], [NaN NaN])  # D test
                      , NaN # KNFST
                      , NaN # SVDD
                    , [NaN], [NaN], [NaN], [NaN], [NaN], [NaN], [NaN] # quantiles
                      , data # data
                      , 0, "None", NaN, false
                         )
  init_detectAnomalies(data, P)
  detectAnomalies!(data, P)
  return(return_detectAnomalies(P))
end


text:
function walk(f, n, i, ::Val{:up})
    if n == 0; return i; end
    @assert 1 <= i <= n

    if heapleft(i) > n  # i has no children
        val = f(i, Int(i>1), 0)
        if val == :break
            return i
        elseif val != :continue
            error("Invalid value leaf($i) = $val encountered in walk_up($n,$i;...)")
        end
        i = heapparent(i)
    end

    if heapright(i) > n  # i has one child
        val = f(i, Int(i>1), 1)
        if val == :break
            return i
        elseif val != :continue
            error("Invalid value border($i) = $val encountered in walk_up($n,$i;...)")
        end
        i = heapparent(i)
    end

    while i > 1
        val = f(i,1,2)
        if val == :break
            return i
        elseif val != :continue
            error("Invalid value inner($i) = $val encountered in walk_up($n,$i;...)")
        end
        i = heapparent(i)
    end

    if i > 0
        f(i,0, min(n-1,2))
    end
    return 1
end

function walk(f, n, i, ::Val{:down})
    if n == 0; return i; end
    @assert 1 <= i <= n
    while heapright(i) <= n
        val = f(i, Int(i>1), 2)
        if val == :left
            i = heapleft(i)
        elseif val == :right
            i = heapright(i)
        elseif val == :break
            return i
        else
            error("Invalid value inner($i) = $val encountered in walk_down($n,$i;...)")
        end
    end
    if heapleft(i) <= n
        val = f(i, Int(i>1), 1)
        if val == :break
            return i
        elseif val != :continue
            error("Invalid value border($i) = $val encountered in walk_down($n,$i;...)")
        end
        i = heapleft(i)
    end
    val = f(i, Int(i>1), 0)
    return i
end

function piggyback(primary, secondaries...)
    return (args...) -> begin
        val = primary(args...)
        for secondary in secondaries
            secondary(args...)
        end
        return val
    end
end

ext: jl


text:
"""
    Quintic{T}

The scalar conservation law with flux
``
  \\partial_t u + \\partial_x u^5 = 0
``
in one space dimensions using `T` as scalar type.
"""
struct Quintic{T} <: ScalarBalanceLaw{T,1} end

function Quintic(T=Float64)
  Quintic{T}()
end

function Base.show(io::IO, model::Quintic{T}) where {T}
  print(io, "Scalar conservation law {T=", T, "} with flux f(u) = u^5")
end

"""
    flux(u, model::Quintic)

Compute the flux of `u` for `model`.
"""
@inline flux(u, model::Quintic) = u^2*u^3

"""
    speed(u::Real, model::Quintic)

Compute the speed f'(`u`) for `model`.
"""
@inline speed(u, model::Quintic) = 5*(u^2)^2

################################################################################

"""
    (::GodunovFlux)(uₗ::T, uᵣ::T, model::Quintic{T}) where {T}

Compute Godunov's flux between `uₗ` and `uᵣ` for `model`.
"""
function (::GodunovFlux)(uₗ::T, uᵣ::T, model::Quintic{T}) where {T}
  flux(uₗ, model)
end

"""
    (::EnergyConservativeFlux)(uₗ::T, uᵣ::T, model::Quintic{T}) where {T}

Compute the energy (L₂ entropy) conservative flux between `uₗ` and `uᵣ` for `model`.
"""
function (flux::EnergyConservativeFlux)(uₗ::T, uᵣ::T, model::Quintic{T}) where T<:Real
    (uₗ^2*uₗ^3 + (uₗ^2)^2*uᵣ + uₗ^3*uᵣ^2 + uₗ^2*uᵣ^3 + uₗ*(uᵣ^2)^2 + uᵣ^2*uᵣ^3) / 6
end

################################################################################

@inline function add_numerical_fluxes_inner_loop2!(du, fluxes, u, balance_law::Quintic,
                                                    Nx, basis, boundaries_included::Val{false},
                                                    jacx, parallel)
    Pp1 = length(basis.nodes)
    Rl = basis.interp_left
    Rr = basis.interp_right
    one_3 = 1 / 3
    one_6 = 1 / 6

    # add numerical fluxes
    @inbounds for ix in Base.OneTo(Nx)
        Rul = zero(eltype(u))
        Rur = zero(eltype(u))
        for nx in Base.OneTo(Pp1)
            Rul += Rl[nx]*u[nx,ix]
            Rur += Rr[nx]*u[nx,ix]
        end
        Ru2l = zero(eltype(u))
        Ru2r = zero(eltype(u))
        for nx in Base.OneTo(Pp1)
            tmp = u[nx,ix]^2
            Ru2l += Rl[nx]*tmp
            Ru2r += Rr[nx]*tmp
        end
        Ru3l = zero(eltype(u))
        Ru3r = zero(eltype(u))
        for nx in Base.OneTo(Pp1)
            tmp = u[nx,ix]^3
            Ru3l += Rl[nx]*tmp
            Ru3r += Rr[nx]*tmp
        end
        Ru4l = zero(eltype(u))
        Ru4r = zero(eltype(u))
        for nx in Base.OneTo(Pp1)
            tmp = (u[nx,ix]^2)^2
            Ru4l += Rl[nx]*tmp
            Ru4r += Rr[nx]*tmp
        end
        Ru5l = zero(eltype(u))
        Ru5r = zero(eltype(u))
        for nx in Base.OneTo(Pp1)
            tmp = u[nx,ix]^2*u[nx,ix]^3
            Ru5l += Rl[nx]*tmp
            Ru5r += Rr[nx]*tmp
        end

        for nx in Base.OneTo(Pp1)
            du[nx,ix] += (  (fluxes[ix  ]   - one_3 * Ru5l
                                            - one_3 * u[nx,ix] * Ru4l
                                            - one_6 * u[nx,ix]^2 * Ru3l
                                            - one_6 * Rul^3*Rul^2
                                            + one_6 * u[nx,ix] * (Rul^2)^2
                                            - one_6 * Ru2l * Rul^3
                                            + one_6 * u[nx,ix]^2 * Rul^3
                                            - one_6 * Ru3l * Rul^2
                                            + one_6 * u[nx,ix] * Ru3l * Rul
                                            - one_6 * Ru3l * Ru2l) * Rl[nx]
                          - (fluxes[ix+1]   - one_3 * Ru5r
                                            - one_3 * u[nx,ix] * Ru4r
                                            - one_6 * u[nx,ix]^2 * Ru3r
                                            - one_6 * Rur^3*Rur^2
                                            + one_6 * u[nx,ix] * (Rur^2)^2
                                            - one_6 * Ru2r * Rur^3
                                            + one_6 * u[nx,ix]^2 * Rur^3
                                            - one_6 * Ru3r * Rur^2
                                            + one_6 * u[nx,ix] * Ru3r * Rur
                                            - one_6 * Ru3r * Ru2r) * Rr[nx]
                            ) * jacx / basis.weights[nx]
        end
    end

    nothing
end


text:
type MpcModel
    mdl::JuMP.Model

    z0::Array{JuMP.NonlinearParameter,1}
    coeff::Array{JuMP.NonlinearParameter,1}
    c_Vx::Array{JuMP.NonlinearParameter,1}
    c_Vy::Array{JuMP.NonlinearParameter,1}
    c_Psi::Array{JuMP.NonlinearParameter,1}
    coeffTermConst::Array{JuMP.NonlinearParameter,3}
    coeffTermCost::Array{JuMP.NonlinearParameter,2}

    z_Ol::Array{JuMP.Variable,2}
    u_Ol::Array{JuMP.Variable,2}
    ParInt::JuMP.Variable

    laneCost::JuMP.NonlinearExpression
    constZTerm::JuMP.NonlinearExpression
    costZTerm::JuMP.NonlinearExpression
    derivCost::JuMP.NonlinearExpression
    controlCost::JuMP.NonlinearExpression
    costZ::JuMP.NonlinearExpression

    uPrev::Array{JuMP.NonlinearParameter,2}

    function MpcModel(mpcParams::MpcParams,mpcCoeff::MpcCoeff,modelParams::ModelParams,trackCoeff::TrackCoeff)
        m = new()
        dt   = modelParams.dt
        L_a  = modelParams.l_A
        L_b  = modelParams.l_B
        c0   = modelParams.c0
        u_lb = modelParams.u_lb
        u_ub = modelParams.u_ub
        z_lb = modelParams.z_lb
        z_ub = modelParams.z_ub

        N               = mpcParams.N
        Q               = mpcParams.Q
        Q_term          = mpcParams.Q_term
        R               = mpcParams.R
        order           = mpcCoeff.order       # polynomial order of terminal constraints and cost approximation
        ey_max          = trackCoeff.width/2

        QderivZ         = mpcParams.QderivZ::Array{Float64,1}
        QderivU         = mpcParams.QderivU::Array{Float64,1}
        Q_term_cost     = mpcParams.Q_term_cost::Float64
        delay_df        = mpcParams.delay_df
        delay_a         = mpcParams.delay_a

        acc_f           = 1.0

        n_poly_curv = trackCoeff.nPolyCurvature         # polynomial degree of curvature approximation

        # Path following mode:
        # Create function-specific parameters
        v_ref       = mpcParams.vPathFollowing
        z_Ref::Array{Float64,2}
        z_Ref       = cat(2,v_ref*ones(N+1,1),zeros(N+1,5))       # Reference trajectory: path following -> stay on line and keep constant velocity
        u_Ref       = zeros(N,2)

        mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.09))#,check_derivatives_for_naninf="yes"))#,linear_solver="ma57",print_user_options="yes"))

        @variable( mdl, z_Ol[1:(N+1),1:7])
        @variable( mdl, u_Ol[1:N,1:2])
        @variable( mdl, 0 <= ParInt <= 1)
        #@variable( mdl, eps[1:2] >= 0) # eps for soft lane constraints
        @variable( mdl, eps[1:N+1] >= 0) # eps for soft lane constraints

        z_lb_6s = ones(mpcParams.N+1,1)*[0.1 -Inf -Inf -Inf -Inf -Inf -Inf]                      # lower bounds on states
        z_ub_6s = ones(mpcParams.N+1,1)*[3.5  Inf Inf  Inf  Inf  Inf Inf]                      # upper bounds
        u_lb_6s = ones(mpcParams.N,1) * [-1.0  -0.3]                                         # lower bounds on steering
        u_ub_6s = ones(mpcParams.N,1) * [2.0   0.3]                                         # upper bounds

        for i=1:2
            for j=1:N
                setlowerbound(u_Ol[j,i], u_lb_6s[j,i])
                setupperbound(u_Ol[j,i], u_ub_6s[j,i])
            end
        end
        for i=1:7
            for j=1:N+1
                setlowerbound(z_Ol[j,i], z_lb_6s[j,i])
                setupperbound(z_Ol[j,i], z_ub_6s[j,i])
            end
        end

        @NLparameter(mdl, z0[i=1:7] == 0)
        @NLparameter(mdl, coeff[i=1:n_poly_curv+1] == 0)
        @NLparameter(mdl, c_Vx[i=1:3]  == 0)
        @NLparameter(mdl, c_Vy[i=1:4]  == 0)
        @NLparameter(mdl, c_Psi[i=1:3] == 0)
        @NLparameter(mdl, coeffTermConst[i=1:order+1,j=1:2,k=1:5] == 0)
        @NLparameter(mdl, coeffTermCost[i=1:order+1,j=1:2] == 0)
        @NLparameter(mdl, uPrev[1:10,1:2] == 0)

        # Conditions for first solve:
        setvalue(z0[1],1)
        setvalue(c_Vx[3],0.1)

        @NLconstraint(mdl, [i=1:7], z_Ol[1,i] == z0[i])
        #@NLconstraint(mdl, [i=1:N+1], z_Ol[i,5] <=  ey_max + eps[1])
        #@NLconstraint(mdl, [i=1:N+1], z_Ol[i,5] >= -ey_max - eps[2])
        @NLconstraint(mdl, [i=1:N+1], z_Ol[i,5] <= ey_max + eps[i])
        @NLconstraint(mdl, [i=1:N+1], z_Ol[i,5] >= -ey_max - eps[i])

        @NLexpression(mdl, c[i = 1:N], sum{coeff[j]*z_Ol[i,6]^(n_poly_curv-j+1),j=1:n_poly_curv} + coeff[n_poly_curv+1])
        @NLexpression(mdl, dsdt[i = 1:N], (z_Ol[i,1]*cos(z_Ol[i,4]) - z_Ol[i,2]*sin(z_Ol[i,4]))/(1-z_Ol[i,5]*c[i]))

        println("Initializing model...")

        # System dynamics
        for i=1:N
            if i<=delay_df
                @NLconstraint(mdl, z_Ol[i+1,2]  == z_Ol[i,2] + c_Vy[1]*z_Ol[i,2]/z_Ol[i,1] + c_Vy[2]*z_Ol[i,1]*z_Ol[i,3] + c_Vy[3]*z_Ol[i,3]/z_Ol[i,1] + c_Vy[4]*uPrev[delay_df+1-i,2]) # yDot
                @NLconstraint(mdl, z_Ol[i+1,3]  == z_Ol[i,3] + c_Psi[1]*z_Ol[i,3]/z_Ol[i,1] + c_Psi[2]*z_Ol[i,2]/z_Ol[i,1] + c_Psi[3]*uPrev[delay_df+1-i,2])                            # psiDot
            else
                @NLconstraint(mdl, z_Ol[i+1,2]  == z_Ol[i,2] + c_Vy[1]*z_Ol[i,2]/z_Ol[i,1] + c_Vy[2]*z_Ol[i,1]*z_Ol[i,3] + c_Vy[3]*z_Ol[i,3]/z_Ol[i,1] + c_Vy[4]*u_Ol[i-delay_df,2]) # yDot
                @NLconstraint(mdl, z_Ol[i+1,3]  == z_Ol[i,3] + c_Psi[1]*z_Ol[i,3]/z_Ol[i,1] + c_Psi[2]*z_Ol[i,2]/z_Ol[i,1] + c_Psi[3]*u_Ol[i-delay_df,2])                            # psiDot
            end
            if i<=delay_a
                #@NLconstraint(mdl, z_Ol[i+1,1]  == z_Ol[i,1] + dt*(uPrev[delay_a+1-i,1] - 0.5*z_Ol[i,1]))
                #@NLconstraint(mdl, z_Ol[i+1,1]  == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2] + c_Vx[2]*z_Ol[i,3] + c_Vx[3]*z_Ol[i,1] + c_Vx[4]*u_Ol[i,1])                              # xDot
                @NLconstraint(mdl, z_Ol[i+1,7]  == z_Ol[i,7] + dt*(uPrev[delay_a+1-i,1]-z_Ol[i,7])*acc_f)
            else
                #@NLconstraint(mdl, z_Ol[i+1,1]  == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2] + c_Vx[2]*z_Ol[i,3] + c_Vx[3]*z_Ol[i,1] + c_Vx[4]*u_Ol[i,1])                              # xDot
                @NLconstraint(mdl, z_Ol[i+1,7]  == z_Ol[i,7] + dt*(u_Ol[i-delay_a,1]-z_Ol[i,7])*acc_f)
            end
            #@NLconstraint(mdl, z_Ol[i+1,1]  == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2] + c_Vx[2]*z_Ol[i,3] + c_Vx[3]*z_Ol[i,1] + c_Vx[4]*z_Ol[i,7])                               # xDot
            #@NLconstraint(mdl, z_Ol[i+1,1]  == z_Ol[i,1] + dt*(z_Ol[i,7] - 0.5*z_Ol[i,1]))                               # xDot
            @NLconstraint(mdl, z_Ol[i+1,1]  == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2]*z_Ol[i,3] + c_Vx[2]*z_Ol[i,1] + c_Vx[3]*z_Ol[i,7])
            @NLconstraint(mdl, z_Ol[i+1,4]  == z_Ol[i,4] + dt*(z_Ol[i,3]-dsdt[i]*c[i]))                                                                                # ePsi
            @NLconstraint(mdl, z_Ol[i+1,5]  == z_Ol[i,5] + dt*(z_Ol[i,1]*sin(z_Ol[i,4])+z_Ol[i,2]*cos(z_Ol[i,4])))                                                      # eY
            @NLconstraint(mdl, z_Ol[i+1,6]  == z_Ol[i,6] + dt*dsdt[i]  )                                                                                # s
        end
        # @NLconstraint(mdl, u_Ol[1,1]-uPrev[1,1] <= 0.05)
        # @NLconstraint(mdl, u_Ol[1,1]-uPrev[1,1] >= -0.2)
        # for i=1:N-1 # Constraints on u:
        #     @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] <= 0.05)
        #     @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] >= -0.2)
        # end

        @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] <= 0.06)
        @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] >= -0.06)
        for i=1:N-1 # Constraints on u:
            @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] <= 0.06)
            @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] >= -0.06)
        end

        # Cost functions

        # Derivative cost
        # ---------------------------------
        @NLexpression(mdl, derivCost, sum{QderivZ[j]*(sum{(z_Ol[i,j]-z_Ol[i+1,j])^2,i=1:N}),j=1:6} +
                                          QderivU[1]*((uPrev[1,1]-u_Ol[1,1])^2+sum{(u_Ol[i,1]-u_Ol[i+1,1])^2,i=1:N-delay_a-1})+
                                          QderivU[2]*((uPrev[1,2]-u_Ol[1,2])^2+sum{(u_Ol[i,2]-u_Ol[i+1,2])^2,i=1:N-delay_df-1}))

        # Lane cost
        # ---------------------------------
        #@NLexpression(mdl, laneCost, sum{100000*eps[i]+1000*eps[i]^2,i=1:2})
        @NLexpression(mdl, laneCost, sum{10*eps[i]+100*eps[i]^2,i=2:N+1})

        # Lane cost
        # ---------------------------------
        #@NLexpression(mdl, laneCost, 100*sum{z_Ol[i,5]^2*((0.5+0.5*tanh(10*(z_Ol[i,5]-ey_max))) + (0.5-0.5*tanh(10*(z_Ol[i,5]+ey_max)))),i=1:N+1})

        # Control Input cost
        # ---------------------------------
        @NLexpression(mdl, controlCost, R[1]*sum{(u_Ol[i,1])^2,i=1:N-delay_a}+
                                        R[2]*sum{(u_Ol[i,2])^2,i=1:N-delay_df})

        # Terminal constraints (soft), starting from 2nd lap
        # ---------------------------------
        @NLexpression(mdl, constZTerm, sum{Q_term[j]*(ParInt*(sum{coeffTermConst[i,1,j]*z_Ol[N+1,6]^(order+1-i),i=1:order}+coeffTermConst[order+1,1,j])+
                                            (1-ParInt)*(sum{coeffTermConst[i,2,j]*z_Ol[N+1,6]^(order+1-i),i=1:order}+coeffTermConst[order+1,2,j])-z_Ol[N+1,j])^2,j=1:5})

        # Terminal cost
        # ---------------------------------
        # The value of this cost determines how fast the algorithm learns. The higher this cost, the faster the control tries to reach the finish line.
        @NLexpression(mdl, costZTerm, 1/5*(Q_term_cost*(ParInt*(sum{coeffTermCost[i,1]*z_Ol[N+1,6]^(order+1-i),i=1:order}+coeffTermCost[order+1,1])+
                                      (1-ParInt)*(sum{coeffTermCost[i,2]*z_Ol[N+1,6]^(order+1-i),i=1:order}+coeffTermCost[order+1,2]))))

        # State cost (only for path following mode)
        # ---------------------------------
        #@NLexpression(mdl, costZ, 0.5*sum{Q[i]*sum{(z_Ol[j,i]-z_Ref[j,i])^2,j=1:N+1},i=1:6})    # Follow trajectory
        #@NLexpression(mdl, costZ, 0.5*sum{(Q[1]*(sqrt(z_Ol[j,1]^2+z_Ol[j,2]^2)-1.0)^2 + Q[4]*z_Ol[j,4]^2 + Q[5]*z_Ol[j,5]^2),j=2:N+1})

        @NLexpression(mdl, costZ, 0*QderivU[1]*sum{z_Ol[i,7],i=1:N})
        # Solve model once
        @NLobjective(mdl, Min, derivCost + constZTerm + costZTerm + laneCost + N*Q_term_cost)
        #@NLobjective(mdl, Min, derivCost + costZ)
        sol_stat=solve(mdl)
        println("Finished solve 1: $sol_stat")
        sol_stat=solve(mdl)
        println("Finished solve 2: $sol_stat")
        m.mdl = mdl
        m.z0 = z0
        m.coeff = coeff
        m.z_Ol = z_Ol
        m.u_Ol = u_Ol
        m.c_Vx = c_Vx
        m.c_Vy = c_Vy
        m.c_Psi = c_Psi
        m.ParInt = ParInt
        m.uPrev = uPrev

        m.coeffTermCost = coeffTermCost
        m.coeffTermConst = coeffTermConst

        m.derivCost = derivCost
        m.controlCost = controlCost
        m.laneCost = laneCost
        m.constZTerm = constZTerm
        m.costZTerm  = costZTerm
        m.costZ  = costZ
        return m
    end
end

type MpcModel_pF
    mdl::JuMP.Model

    z0::Array{JuMP.NonlinearParameter,1}
    coeff::Array{JuMP.NonlinearParameter,1}
    z_Ref::Array{JuMP.NonlinearParameter,2}

    z_Ol::Array{JuMP.Variable,2}
    u_Ol::Array{JuMP.Variable,2}

    derivCost::JuMP.NonlinearExpression
    costZ::JuMP.NonlinearExpression
    controlCost::JuMP.NonlinearExpression

    uPrev::Array{JuMP.NonlinearParameter,2}

    function MpcModel_pF(mpcParams_pF::MpcParams,modelParams::ModelParams,trackCoeff::TrackCoeff)
        println("Starting creation of pf model")
        m = new()
        dt          = modelParams.dt
        L_a         = modelParams.l_A
        L_b         = modelParams.l_B
        c0          = modelParams.c0
        u_lb        = modelParams.u_lb
        u_ub        = modelParams.u_ub
        z_lb        = modelParams.z_lb
        z_ub        = modelParams.z_ub

        N           = mpcParams_pF.N
        Q           = mpcParams_pF.Q
        R           = mpcParams_pF.R
        QderivZ     = mpcParams_pF.QderivZ::Array{Float64,1}
        QderivU     = mpcParams_pF.QderivU::Array{Float64,1}
        delay_df    = mpcParams_pF.delay_df::Int64
        delay_a     = mpcParams_pF.delay_a::Int64

        println("prediction h= ",N)

        v_ref       = mpcParams_pF.vPathFollowing

        acc_f       = 1.0

        n_poly_curv = trackCoeff.nPolyCurvature         # polynomial degree of curvature approximation

        # Create function-specific parameters
        z_ref::Array{Float64,2}
        z_ref       = cat(2,zeros(N+1,3),v_ref*ones(N+1,1))       # Reference trajectory: path following -> stay on line and keep constant velocity
        u_Ref       = zeros(N,2)

        # Create Model
        mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.09))#,linear_solver="ma57",print_user_options="yes"))

        # Create variables (these are going to be optimized)
        @variable( mdl, z_Ol[1:(N+1),1:5], start = 0)          # z = s, ey, epsi, v
        @variable( mdl, u_Ol[1:N,1:2], start = 0)

        # Set bounds
        z_lb_4s = ones(mpcParams_pF.N+1,1)*[-Inf -Inf -Inf -0.5]                  # lower bounds on states
        z_ub_4s = ones(mpcParams_pF.N+1,1)*[ Inf  Inf  Inf  1.5]                  # upper bounds
        u_lb_4s = ones(mpcParams_pF.N,1) * [0  -0.3]                            # lower bounds on steering
        u_ub_4s = ones(mpcParams_pF.N,1) * [1.2   0.3]                            # upper bounds

        for i=1:2
            for j=1:N
                setlowerbound(u_Ol[j,i], u_lb_4s[j,i])
                setupperbound(u_Ol[j,i], u_ub_4s[j,i])
            end
        end
        # for i=1:4
        #     for j=1:N+1
        #         setlowerbound(z_Ol[j,i], z_lb_4s[j,i])
        #         setupperbound(z_Ol[j,i], z_ub_4s[j,i])
        #     end
        # end

        @NLparameter(mdl, z_Ref[1:N+1,1:4] == 0)
        @NLparameter(mdl, z0[i=1:5] == 0)
        @NLparameter(mdl, uPrev[1:10,1:2] == 0)

        @NLparameter(mdl, coeff[i=1:n_poly_curv+1] == 0)

        @NLexpression(mdl, c[i = 1:N], sum{coeff[j]*z_Ol[i,1]^(n_poly_curv-j+1),j=1:n_poly_curv} + coeff[n_poly_curv+1])

        # System dynamics
        setvalue(z0[4],v_ref)
        @NLconstraint(mdl, [i=1:5], z_Ol[1,i] == z0[i])         # initial condition
        for i=1:N
            if i<=delay_df
                @NLexpression(mdl, bta[i],  atan( L_a / (L_a + L_b) * tan( uPrev[delay_df+1-i,2] ) ) )
            else
                @NLexpression(mdl, bta[i],  atan( L_a / (L_a + L_b) * tan( u_Ol[i-delay_df,2] ) ) )
            end
            if i<=delay_a
                #@NLconstraint(mdl, z_Ol[i+1,4] == z_Ol[i,4] + dt*(uPrev[delay_a+1-i,1] - 0.5*z_Ol[i,4]))  # v
                @NLconstraint(mdl, z_Ol[i+1,5] == z_Ol[i,5] + dt*(uPrev[delay_a+1-i,1] - z_Ol[i,5])*acc_f)  # v
            else
                #@NLconstraint(mdl, z_Ol[i+1,4] == z_Ol[i,4] + dt*(u_Ol[i-delay_a,1] - 0.5*z_Ol[i,4]))     # v
                @NLconstraint(mdl, z_Ol[i+1,5] == z_Ol[i,5] + dt*(u_Ol[i-delay_a,1] - z_Ol[i,5])*acc_f)     # v
            end

            @NLexpression(mdl, dsdt[i], z_Ol[i,4]*cos(z_Ol[i,3]+bta[i])/(1-z_Ol[i,2]*c[i]))
            @NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + dt*dsdt[i]  )                                                # s
            @NLconstraint(mdl, z_Ol[i+1,2] == z_Ol[i,2] + dt*z_Ol[i,4]*sin(z_Ol[i,3]+bta[i])  )                        # ey
            @NLconstraint(mdl, z_Ol[i+1,3] == z_Ol[i,3] + dt*(z_Ol[i,4]/L_a*sin(bta[i])-dsdt[i]*c[i])  )               # epsi
            @NLconstraint(mdl, z_Ol[i+1,4] == z_Ol[i,4] + dt*(z_Ol[i,5] - 0.5*z_Ol[i,4]))  # v
        end

        # Cost definitions
        # Derivative cost
        # ---------------------------------
        @NLexpression(mdl, derivCost, sum{QderivZ[j]*(sum{(z_Ol[i,j]-z_Ol[i+1,j])^2,i=1:N}),j=1:4} +
                                            QderivU[1]*((uPrev[1,1]-u_Ol[1,1])^2+sum{(u_Ol[i,1]-u_Ol[i+1,1])^2,i=1:N-delay_a-1})+
                                            QderivU[2]*((uPrev[1,2]-u_Ol[1,2])^2+sum{(u_Ol[i,2]-u_Ol[i+1,2])^2,i=1:N-delay_df-1}))

        # Control Input cost
        # ---------------------------------
        @NLexpression(mdl, controlCost, 0.5*R[1]*sum{(u_Ol[i,1])^2,i=1:N-delay_a}+
                                        0.5*R[2]*sum{(u_Ol[i,2])^2,i=1:N-delay_df})

        # State cost
        # ---------------------------------
        @NLexpression(mdl, costZ, 0.5*sum{Q[i]*sum{(z_Ol[j,i]-z_Ref[j,i])^2,j=2:N+1},i=1:4})    # Follow trajectory

        # Objective function
        @NLobjective(mdl, Min, costZ + derivCost + controlCost)

        # create first artificial solution (for warm start)
        for i=1:N+1
            setvalue(z_Ol[i,:],[(i-1)*dt*v_ref 0 0 v_ref 0])
        end
        for i=1:N
            setvalue(u_Ol[i,:],[0.15 0])
        end
        # First solve
        sol_stat=solve(mdl)
        println("Finished solve 1: $sol_stat")
        sol_stat=solve(mdl)
        println("Finished solve 2: $sol_stat")

        m.mdl = mdl
        m.z0 = z0
        m.z_Ref = z_Ref
        m.coeff = coeff
        m.z_Ol = z_Ol
        m.u_Ol = u_Ol
        m.uPrev = uPrev
        m.derivCost = derivCost
        m.costZ = costZ
        m.controlCost = controlCost
        return m
    end
end

type MpcModel_convhull

    mdl::JuMP.Model

    z0::Array{JuMP.NonlinearParameter,1}
    coeff::Array{JuMP.NonlinearParameter,1}
    selStates::Array{JuMP.NonlinearParameter,2}
    statesCost::Array{JuMP.NonlinearParameter,1}
    c_Vx::Array{JuMP.NonlinearParameter,1}
    c_Vy::Array{JuMP.NonlinearParameter,1}
    c_Psi::Array{JuMP.NonlinearParameter,1}
    uPrev::Array{JuMP.NonlinearParameter,2}

    eps_lane::Array{JuMP.Variable,1}
    #eps_alpha::Array{JuMP.Variable,1}
    #eps_vel::Array{JuMP.Variable,1}
    alpha::Array{JuMP.Variable,1}
    z_Ol::Array{JuMP.Variable,2}
    u_Ol::Array{JuMP.Variable,2}

    dsdt::Array{JuMP.NonlinearExpression,1}
    c::Array{JuMP.NonlinearExpression,1}

    derivCost::JuMP.NonlinearExpression
    controlCost::JuMP.NonlinearExpression
    laneCost::JuMP.NonlinearExpression
    terminalCost::JuMP.NonlinearExpression
    slackVx::JuMP.NonlinearExpression
    slackVy::JuMP.NonlinearExpression
    slackPsidot::JuMP.NonlinearExpression
    slackEpsi::JuMP.NonlinearExpression
    slackEy::JuMP.NonlinearExpression
    slackS::JuMP.NonlinearExpression
    #slackCost::JuMP.NonlinearExpression
    #velocityCost::JuMP.NonlinearExpression

    function MpcModel_convhull(mpcParams::MpcParams,mpcCoeff::MpcCoeff,modelParams::ModelParams,trackCoeff::TrackCoeff,selectedStates::SelectedStates)

        m = new()

        #### Initialize parameters

        dt         = modelParams.dt                # time step
        L_a        = modelParams.l_A               # distance from CoM of the car to the front wheels
        L_b        = modelParams.l_B               # distance from CoM of the car to the rear wheels
        u_lb       = modelParams.u_lb              # lower bounds for the control inputs
        u_ub       = modelParams.u_ub              # upper bounds for the control inputs
        z_lb       = modelParams.z_lb              # lower bounds for the states
        z_ub       = modelParams.z_ub              # upper bounds for the states
        c0         = modelParams.c0

        ey_max      = trackCoeff.width/2           # bound for the state ey (distance from the center track). It is set as half of the width of the track for obvious reasons
        n_poly_curv = trackCoeff.nPolyCurvature    # polynomial degree for curvature approximation
        v_max       = 3                            # maximum allowed velocity

        v_ref      = mpcParams.vPathFollowing              # reference velocity for the path following
        N          = mpcParams.N                           # Prediction horizon
        QderivZ    = mpcParams.QderivZ::Array{Float64,1}   # weights for the derivative cost on the states
        QderivU    = mpcParams.QderivU::Array{Float64,1}   # weights for the derivative cost on the control inputs
        R          = mpcParams.R::Array{Float64,1}         # weights on the control inputs
        Q          = mpcParams.Q::Array{Float64,1}         # weights on the states for path following
        Q_lane     = mpcParams.Q_lane::Float64             # weight on the soft constraint on the lane
        #Q_vel      = mpcParams.Q_vel::Float64              # weight on the soft constraint for the max velocity
        delay_df   = mpcParams.delay_df
        delay_a    = mpcParams.delay_a
        Q_slack    = mpcParams.Q_slack

        println("prediction horizon= ",N)

        Np         = selectedStates.Np::Int64              # how many states to select
        Nl         = selectedStates.Nl::Int64              # how many previous laps to select

        acc_f           = 1.0

        mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.09))#,check_derivatives_for_naninf="yes"))#,linear_solver="ma57",print_user_options="yes"))

        @variable( mdl, z_Ol[1:(N+1),1:7])
        @variable( mdl, u_Ol[1:N,1:2])
        @variable( mdl, eps_lane[1:N+1] >= 0)   # eps for soft lane constraints
        @variable( mdl, alpha[1:Nl*Np] >= 0)    # coefficients of the convex hull
       # @variable( mdl, eps_alpha[1:6] >=0)     # eps for soft constraint on alpha
       #@variable( mdl, eps_vel[1:N+1]>=0)      # eps for soft constraint on velocity

        z_lb_6s = ones(mpcParams.N+1,1)*[0.1 -Inf -Inf -Inf -Inf -Inf -Inf]                      # lower bounds on states
        z_ub_6s = ones(mpcParams.N+1,1)*[3.5  Inf Inf  Inf  Inf  Inf Inf]                      # upper bounds
        u_lb_6s = ones(mpcParams.N,1) * [-1.3  -0.3]                                         # lower bounds on steering
        u_ub_6s = ones(mpcParams.N,1) * [2.0   0.3]                                         # upper bounds

        for i=1:2
            for j=1:N
                setlowerbound(u_Ol[j,i], u_lb_6s[j,i])
                setupperbound(u_Ol[j,i], u_ub_6s[j,i])
            end
        end
        for i=1:7
            for j=1:N+1
                setlowerbound(z_Ol[j,i], z_lb_6s[j,i])
                setupperbound(z_Ol[j,i], z_ub_6s[j,i])
            end
        end

        @NLparameter(mdl, z0[i=1:7] == 0)
        @NLparameter(mdl, coeff[i=1:n_poly_curv+1] == 0)
        @NLparameter(mdl, c_Vx[i=1:3]  == 0)
        @NLparameter(mdl, c_Vy[i=1:4]  == 0)
        @NLparameter(mdl, c_Psi[i=1:3] == 0)
        @NLparameter(mdl, uPrev[1:10,1:2] == 0)
        @NLparameter(mdl, selStates[1:Nl*Np,1:6] == 0)                                 # states from the previous trajectories selected in "convhullStates"
        @NLparameter(mdl, statesCost[1:Nl*Np] == 0)                                    # costs of the states selected in "convhullStates"

        # Conditions for first solve:
        setvalue(z0[1],1)
        setvalue(c_Vx[3],0.1)

        @NLconstraint(mdl, [i=1:7], z_Ol[1,i] == z0[i])

        @NLconstraint(mdl, [i=2:N+1], z_Ol[i,5] <= ey_max + eps_lane[i])
        @NLconstraint(mdl, [i=2:N+1], z_Ol[i,5] >= -ey_max - eps_lane[i])
        #@NLconstraint(mdl,[i = 1:(N+1)], z_Ol[i,4] <= v_max + eps_vel[i] )      # soft constraint on maximum velocity
        @NLconstraint(mdl, sum{alpha[i],i=1:Nl*Np} == 1)                        # constraint on the coefficients of the convex hull

        #for n = 1:6
            #@NLconstraint(mdl,z_Ol[N+1,n] == sum{alpha[j]*selStates[j,n],j=1:Nl*Np})  # terminal constraint
            #@NLconstraint(mdl,z_Ol[N+1,n] >= sum{alpha[j]*selStates[j,n],j=1:Nl*Np}-eps_alpha[n])
            #@NLconstraint(mdl,z_Ol[N+1,n] <= sum{alpha[j]*selStates[j,n],j=1:Nl*Np}+eps_alpha[n])
        #end

        @NLexpression(mdl, c[i = 1:N], sum{coeff[j]*z_Ol[i,6]^(n_poly_curv-j+1),j=1:n_poly_curv} + coeff[n_poly_curv+1])
        @NLexpression(mdl, dsdt[i = 1:N], (z_Ol[i,1]*cos(z_Ol[i,4]) - z_Ol[i,2]*sin(z_Ol[i,4]))/(1-z_Ol[i,5]*c[i]))

        println("Initializing model...")

        # System dynamics
        for i=1:N
            if i<=delay_df
                @NLconstraint(mdl, z_Ol[i+1,2]  == z_Ol[i,2] + c_Vy[1]*z_Ol[i,2]/z_Ol[i,1] + c_Vy[2]*z_Ol[i,1]*z_Ol[i,3] + c_Vy[3]*z_Ol[i,3]/z_Ol[i,1] + c_Vy[4]*uPrev[delay_df+1-i,2]) # yDot
                @NLconstraint(mdl, z_Ol[i+1,3]  == z_Ol[i,3] + c_Psi[1]*z_Ol[i,3]/z_Ol[i,1] + c_Psi[2]*z_Ol[i,2]/z_Ol[i,1] + c_Psi[3]*uPrev[delay_df+1-i,2])                            # psiDot
            else
                @NLconstraint(mdl, z_Ol[i+1,2]  == z_Ol[i,2] + c_Vy[1]*z_Ol[i,2]/z_Ol[i,1] + c_Vy[2]*z_Ol[i,1]*z_Ol[i,3] + c_Vy[3]*z_Ol[i,3]/z_Ol[i,1] + c_Vy[4]*u_Ol[i-delay_df,2]) # yDot
                @NLconstraint(mdl, z_Ol[i+1,3]  == z_Ol[i,3] + c_Psi[1]*z_Ol[i,3]/z_Ol[i,1] + c_Psi[2]*z_Ol[i,2]/z_Ol[i,1] + c_Psi[3]*u_Ol[i-delay_df,2])                            # psiDot
            end
            if i<=delay_a
                #@NLconstraint(mdl, z_Ol[i+1,1]  == z_Ol[i,1] + dt*(uPrev[delay_a+1-i,1] - 0.5*z_Ol[i,1]))
                #@NLconstraint(mdl, z_Ol[i+1,1]  == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2] + c_Vx[2]*z_Ol[i,3] + c_Vx[3]*z_Ol[i,1] + c_Vx[4]*u_Ol[i,1])                              # xDot
                @NLconstraint(mdl, z_Ol[i+1,7]  == z_Ol[i,7] + dt*(uPrev[delay_a+1-i,1]-z_Ol[i,7])*acc_f)
            else
                #@NLconstraint(mdl, z_Ol[i+1,1]  == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2] + c_Vx[2]*z_Ol[i,3] + c_Vx[3]*z_Ol[i,1] + c_Vx[4]*u_Ol[i,1])                              # xDot
                @NLconstraint(mdl, z_Ol[i+1,7]  == z_Ol[i,7] + dt*(u_Ol[i-delay_a,1]-z_Ol[i,7])*acc_f)
            end
            #@NLconstraint(mdl, z_Ol[i+1,1]  == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2] + c_Vx[2]*z_Ol[i,3] + c_Vx[3]*z_Ol[i,1] + c_Vx[4]*z_Ol[i,7])                               # xDot
            #@NLconstraint(mdl, z_Ol[i+1,1]  == z_Ol[i,1] + dt*(z_Ol[i,7] - 0.5*z_Ol[i,1]))                               # xDot
            @NLconstraint(mdl, z_Ol[i+1,1]  == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2]*z_Ol[i,3] + c_Vx[2]*z_Ol[i,1] + c_Vx[3]*z_Ol[i,7])
            @NLconstraint(mdl, z_Ol[i+1,4]  == z_Ol[i,4] + dt*(z_Ol[i,3]-dsdt[i]*c[i]))                                                                                # ePsi
            @NLconstraint(mdl, z_Ol[i+1,5]  == z_Ol[i,5] + dt*(z_Ol[i,1]*sin(z_Ol[i,4])+z_Ol[i,2]*cos(z_Ol[i,4])))                                                      # eY
            @NLconstraint(mdl, z_Ol[i+1,6]  == z_Ol[i,6] + dt*dsdt[i]  )                                                                                # s
        end
        # @NLconstraint(mdl, u_Ol[1,1]-uPrev[1,1] <= 0.05)
        # @NLconstraint(mdl, u_Ol[1,1]-uPrev[1,1] >= -0.2)
        # for i=1:N-1 # Constraints on u:
        #     @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] <= 0.05)
        #     @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] >= -0.2)
        # end

        @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] <= 0.06)
        @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] >= -0.06)
        for i=1:N-1 # Constraints on u:
            @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] <= 0.06)
            @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] >= -0.06)
        end

        # Cost functions

        # Derivative cost
        # ---------------------------------
        @NLexpression(mdl, derivCost, sum{QderivZ[j]*(sum{(z_Ol[i,j]-z_Ol[i+1,j])^2,i=1:N}),j=1:6} +
                                          QderivU[1]*((uPrev[1,1]-u_Ol[1,1])^2+sum{(u_Ol[i,1]-u_Ol[i+1,1])^2,i=1:N-1})+
                                          QderivU[2]*((uPrev[1,2]-u_Ol[1,2])^2+sum{(u_Ol[i,2]-u_Ol[i+1,2])^2,i=1:N-delay_df-1}))

        # Control Input cost
        # ---------------------------------
        @NLexpression(mdl, controlCost, R[1]*sum{(u_Ol[i,1])^2,i=1:N-delay_a}+
                                        R[2]*sum{(u_Ol[i,2])^2,i=1:N-delay_df})

        # Lane cost (soft)
        # ---------------------------------
        @NLexpression(mdl, laneCost, Q_lane*sum{40.0*eps_lane[i]+300.0*eps_lane[i]^2 ,i=2:N+1})

        # Terminal Cost
        # ---------------------------------
        @NLexpression(mdl, terminalCost , sum{alpha[i]*statesCost[i], i=1:Nl*Np})

        # Slack cost (soft)
        # ---------------------------------
        #@NLexpression(mdl, slackCost, sum{50*eps_alpha[i]+500*eps_alpha[i]^2,i=1:6})

        # Slack cost on vx
        #----------------------------------
        @NLexpression(mdl, slackVx, (z_Ol[N+1,1] - sum{alpha[j]*selStates[j,1],j=1:Nl*Np})^2)

        # Slack cost on vy
        #----------------------------------
        @NLexpression(mdl, slackVy, (z_Ol[N+1,2] - sum{alpha[j]*selStates[j,2],j=1:Nl*Np})^2)

        # Slack cost on Psi dot
        #----------------------------------
        @NLexpression(mdl, slackPsidot, (z_Ol[N+1,3] - sum{alpha[j]*selStates[j,3],j=1:Nl*Np})^2)

        # Slack cost on ePsi
        #----------------------------------
        @NLexpression(mdl, slackEpsi, (z_Ol[N+1,4] - sum{alpha[j]*selStates[j,4],j=1:Nl*Np})^2)

        # Slack cost on ey
        #----------------------------------
        @NLexpression(mdl, slackEy, (z_Ol[N+1,5] - sum{alpha[j]*selStates[j,5],j=1:Nl*Np})^2)

        # Slack cost on s
        #----------------------------------
        @NLexpression(mdl, slackS, (z_Ol[N+1,6] - sum{alpha[j]*selStates[j,6],j=1:Nl*Np})^2)

        # Velocity Cost
        #----------------------------------
        #@NLexpression(mdl, velocityCost , Q_vel*sum{10.0*eps_vel[i]+100.0*eps_vel[i]^2 ,i=2:N+1})

        # Overall Cost function (objective of the minimization)
        # -----------------------------------------------------

        #@NLobjective(mdl, Min, derivCost + laneCost + controlCost + terminalCost )#+ slackCost)#+ velocityCost)

        @NLobjective(mdl, Min, derivCost + laneCost +  terminalCost + Q_slack[1]*slackVx + Q_slack[2]*slackVy + Q_slack[3]*slackPsidot + Q_slack[4]*slackEpsi + Q_slack[5]*slackEy + Q_slack[6]*slackS) #+ controlCost

        sol_stat=solve(mdl)
        println("Finished solve 1 convhull mpc: $sol_stat")
        sol_stat=solve(mdl)
        println("Finished solve 2 convhull mpc: $sol_stat")

        m.mdl = mdl
        m.z0 = z0
        m.coeff = coeff
        m.z_Ol = z_Ol
        m.u_Ol = u_Ol
        m.c_Vx = c_Vx
        m.c_Vy = c_Vy
        m.c_Psi = c_Psi
        m.uPrev = uPrev
        #m.eps_alpha=eps_alpha

        m.derivCost = derivCost
        m.controlCost = controlCost
        m.laneCost = laneCost
        m.terminalCost= terminalCost # terminal cost
        #m.velocityCost= velocityCost #velocity cost
        m.selStates   = selStates    # selected states
        m.statesCost  = statesCost   # cost of the selected states
        m.alpha       = alpha        # parameters of the convex hull

        m.slackVx     = slackVx
        m.slackVy     = slackVy
        m.slackPsidot = slackPsidot
        m.slackEpsi   = slackEpsi
        m.slackEy     = slackEy
        m.slackS      = slackS

        return m
    end
end

type MpcModel_test

    mdl::JuMP.Model

    z0::Array{JuMP.NonlinearParameter,1}
    coeff::Array{JuMP.NonlinearParameter,1}
    selStates::Array{JuMP.NonlinearParameter,2}
    statesCost::Array{JuMP.NonlinearParameter,1}
    c_Vx::Array{JuMP.NonlinearParameter,1}
    c_Vy::Array{JuMP.NonlinearParameter,1}
    c_Psi::Array{JuMP.NonlinearParameter,1}
    uPrev::Array{JuMP.NonlinearParameter,2}

    eps_lane::Array{JuMP.Variable,1}
    #eps_alpha::Array{JuMP.Variable,1}
    #eps_vel::Array{JuMP.Variable,1}
    alpha::Array{JuMP.Variable,1}
    z_Ol::Array{JuMP.Variable,2}
    u_Ol::Array{JuMP.Variable,2}

    dsdt::Array{JuMP.NonlinearExpression,1}
    c::Array{JuMP.NonlinearExpression,1}

    derivCost::JuMP.NonlinearExpression
    controlCost::JuMP.NonlinearExpression
    laneCost::JuMP.NonlinearExpression
    terminalCost::JuMP.NonlinearExpression
    costPF::JuMP.NonlinearExpression
    #slackCost::JuMP.NonlinearExpression
    #velocityCost::JuMP.NonlinearExpression

    function MpcModel_test(mpcParams::MpcParams,mpcCoeff::MpcCoeff,modelParams::ModelParams,trackCoeff::TrackCoeff,selectedStates::SelectedStates)

        m = new()

        #### Initialize parameters

        dt         = modelParams.dt                # time step
        L_a        = modelParams.l_A               # distance from CoM of the car to the front wheels
        L_b        = modelParams.l_B               # distance from CoM of the car to the rear wheels
        u_lb       = modelParams.u_lb              # lower bounds for the control inputs
        u_ub       = modelParams.u_ub              # upper bounds for the control inputs
        z_lb       = modelParams.z_lb              # lower bounds for the states
        z_ub       = modelParams.z_ub              # upper bounds for the states
        c0         = modelParams.c0

        ey_max      = trackCoeff.width/2           # bound for the state ey (distance from the center track). It is set as half of the width of the track for obvious reasons
        n_poly_curv = trackCoeff.nPolyCurvature    # polynomial degree for curvature approximation
        v_max       = 3                            # maximum allowed velocity

        v_ref      = mpcParams.vPathFollowing              # reference velocity for the path following
        N          = mpcParams.N                           # Prediction horizon
        QderivZ    = mpcParams.QderivZ::Array{Float64,1}   # weights for the derivative cost on the states
        QderivU    = mpcParams.QderivU::Array{Float64,1}   # weights for the derivative cost on the control inputs
        R          = mpcParams.R::Array{Float64,1}         # weights on the control inputs
        Q          = mpcParams.Q::Array{Float64,1}         # weights on the states for path following
        Q_lane     = mpcParams.Q_lane::Float64             # weight on the soft constraint on the lane
        #Q_vel      = mpcParams.Q_vel::Float64              # weight on the soft constraint for the max velocity
        delay_df   = mpcParams.delay_df
        delay_a    = mpcParams.delay_a

        Np         = selectedStates.Np::Int64              # how many states to select
        Nl         = selectedStates.Nl::Int64              # how many previous laps to select

        acc_f           = 1.0

        # Create function-specific parameters
        z_Ref::Array{Float64,2}
        z_Ref       = cat(2,v_ref*ones(N+1,1),zeros(N+1,5))       # Reference trajectory: path following -> stay on line and keep constant velocity
        u_Ref       = zeros(N,2)

        mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.09))#,check_derivatives_for_naninf="yes"))#,linear_solver="ma57",print_user_options="yes"))

        @variable( mdl, z_Ol[1:(N+1),1:7])
        @variable( mdl, u_Ol[1:N,1:2])
        @variable( mdl, eps_lane[1:N+1] >= 0)   # eps for soft lane constraints
        @variable( mdl, alpha[1:Nl*Np] >= 0)    # coefficients of the convex hull
       # @variable( mdl, eps_alpha[1:6] >=0)     # eps for soft constraint on alpha
       #@variable( mdl, eps_vel[1:N+1]>=0)      # eps for soft constraint on velocity

        z_lb_6s = ones(mpcParams.N+1,1)*[0.1 -Inf -Inf -Inf -Inf -Inf -Inf]                      # lower bounds on states
        z_ub_6s = ones(mpcParams.N+1,1)*[3.5  Inf Inf  Inf  Inf  Inf Inf]                      # upper bounds
        u_lb_6s = ones(mpcParams.N,1) * [-1.0  -0.3]                                         # lower bounds on steering
        u_ub_6s = ones(mpcParams.N,1) * [2.0   0.3]                                         # upper bounds

        for i=1:2
            for j=1:N
                setlowerbound(u_Ol[j,i], u_lb_6s[j,i])
                setupperbound(u_Ol[j,i], u_ub_6s[j,i])
            end
        end
        for i=1:7
            for j=1:N+1
                setlowerbound(z_Ol[j,i], z_lb_6s[j,i])
                setupperbound(z_Ol[j,i], z_ub_6s[j,i])
            end
        end

        @NLparameter(mdl, z0[i=1:7] == 0)
        @NLparameter(mdl, coeff[i=1:n_poly_curv+1] == 0)
        @NLparameter(mdl, c_Vx[i=1:3]  == 0)
        @NLparameter(mdl, c_Vy[i=1:4]  == 0)
        @NLparameter(mdl, c_Psi[i=1:3] == 0)
        @NLparameter(mdl, uPrev[1:10,1:2] == 0)
        @NLparameter(mdl, selStates[1:Nl*Np,1:6] == 0)                                 # states from the previous trajectories selected in "convhullStates"
        @NLparameter(mdl, statesCost[1:Nl*Np] == 0)                                    # costs of the states selected in "convhullStates"

        # Conditions for first solve:
        setvalue(z0[1],1)
        setvalue(c_Vx[3],0.1)

        @NLconstraint(mdl, [i=1:7], z_Ol[1,i] == z0[i])

        @NLconstraint(mdl, [i=2:N+1], z_Ol[i,5] <= ey_max + eps_lane[i])
        @NLconstraint(mdl, [i=2:N+1], z_Ol[i,5] >= -ey_max - eps_lane[i])
        #@NLconstraint(mdl,[i = 1:(N+1)], z_Ol[i,4] <= v_max + eps_vel[i] )      # sof constraint on maximum velocity
        @NLconstraint(mdl, sum{alpha[i],i=1:Nl*Np} == 1)                        # constraint on the coefficients of the convex hull

        for i = 1:6
            @NLconstraint(mdl,z_Ol[N+1,i] == sum{alpha[j]*selStates[j,i],j=1:Nl*Np})  # terminal constraint
            #@NLconstraint(mdl,z_Ol[N+1,n] >= sum{alpha[j]*selStates[j,n],j=1:Nl*Np}-eps_alpha[n])
            #@NLconstraint(mdl,z_Ol[N+1,n] <= sum{alpha[j]*selStates[j,n],j=1:Nl*Np}+eps_alpha[n])
        end

        @NLexpression(mdl, c[i = 1:N], sum{coeff[j]*z_Ol[i,6]^(n_poly_curv-j+1),j=1:n_poly_curv} + coeff[n_poly_curv+1])
        @NLexpression(mdl, dsdt[i = 1:N], (z_Ol[i,1]*cos(z_Ol[i,4]) - z_Ol[i,2]*sin(z_Ol[i,4]))/(1-z_Ol[i,5]*c[i]))

        println("Initializing model...")

        # System dynamics
        for i=1:N
            if i<=delay_df
                @NLconstraint(mdl, z_Ol[i+1,2]  == z_Ol[i,2] + c_Vy[1]*z_Ol[i,2]/z_Ol[i,1] + c_Vy[2]*z_Ol[i,1]*z_Ol[i,3] + c_Vy[3]*z_Ol[i,3]/z_Ol[i,1] + c_Vy[4]*uPrev[delay_df+1-i,2]) # yDot
                @NLconstraint(mdl, z_Ol[i+1,3]  == z_Ol[i,3] + c_Psi[1]*z_Ol[i,3]/z_Ol[i,1] + c_Psi[2]*z_Ol[i,2]/z_Ol[i,1] + c_Psi[3]*uPrev[delay_df+1-i,2])                            # psiDot
            else
                @NLconstraint(mdl, z_Ol[i+1,2]  == z_Ol[i,2] + c_Vy[1]*z_Ol[i,2]/z_Ol[i,1] + c_Vy[2]*z_Ol[i,1]*z_Ol[i,3] + c_Vy[3]*z_Ol[i,3]/z_Ol[i,1] + c_Vy[4]*u_Ol[i-delay_df,2]) # yDot
                @NLconstraint(mdl, z_Ol[i+1,3]  == z_Ol[i,3] + c_Psi[1]*z_Ol[i,3]/z_Ol[i,1] + c_Psi[2]*z_Ol[i,2]/z_Ol[i,1] + c_Psi[3]*u_Ol[i-delay_df,2])                            # psiDot
            end
            if i<=delay_a
                #@NLconstraint(mdl, z_Ol[i+1,1]  == z_Ol[i,1] + dt*(uPrev[delay_a+1-i,1] - 0.5*z_Ol[i,1]))
                #@NLconstraint(mdl, z_Ol[i+1,1]  == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2] + c_Vx[2]*z_Ol[i,3] + c_Vx[3]*z_Ol[i,1] + c_Vx[4]*u_Ol[i,1])                              # xDot
                @NLconstraint(mdl, z_Ol[i+1,7]  == z_Ol[i,7] + dt*(uPrev[delay_a+1-i,1]-z_Ol[i,7])*acc_f)
            else
                #@NLconstraint(mdl, z_Ol[i+1,1]  == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2] + c_Vx[2]*z_Ol[i,3] + c_Vx[3]*z_Ol[i,1] + c_Vx[4]*u_Ol[i,1])                              # xDot
                @NLconstraint(mdl, z_Ol[i+1,7]  == z_Ol[i,7] + dt*(u_Ol[i-delay_a,1]-z_Ol[i,7])*acc_f)
            end
            #@NLconstraint(mdl, z_Ol[i+1,1]  == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2] + c_Vx[2]*z_Ol[i,3] + c_Vx[3]*z_Ol[i,1] + c_Vx[4]*z_Ol[i,7])                               # xDot
            #@NLconstraint(mdl, z_Ol[i+1,1]  == z_Ol[i,1] + dt*(z_Ol[i,7] - 0.5*z_Ol[i,1]))                               # xDot
            @NLconstraint(mdl, z_Ol[i+1,1]  == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2]*z_Ol[i,3] + c_Vx[2]*z_Ol[i,1] + c_Vx[3]*z_Ol[i,7])
            @NLconstraint(mdl, z_Ol[i+1,4]  == z_Ol[i,4] + dt*(z_Ol[i,3]-dsdt[i]*c[i]))                                                                                # ePsi
            @NLconstraint(mdl, z_Ol[i+1,5]  == z_Ol[i,5] + dt*(z_Ol[i,1]*sin(z_Ol[i,4])+z_Ol[i,2]*cos(z_Ol[i,4])))                                                      # eY
            @NLconstraint(mdl, z_Ol[i+1,6]  == z_Ol[i,6] + dt*dsdt[i]  )                                                                                # s
        end
        @NLconstraint(mdl, u_Ol[1,1]-uPrev[1,1] <= 0.05)
        @NLconstraint(mdl, u_Ol[1,1]-uPrev[1,1] >= -0.2)
        for i=1:N-1 # Constraints on u:
            @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] <= 0.05)
            @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] >= -0.2)
        end

        @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] <= 0.06)
        @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] >= -0.06)
        for i=1:N-1 # Constraints on u:
            @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] <= 0.06)
            @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] >= -0.06)
        end

        # Cost functions

        # Derivative cost
        # ---------------------------------
        @NLexpression(mdl, derivCost, sum{QderivZ[j]*(sum{(z_Ol[i,j]-z_Ol[i+1,j])^2,i=1:N}),j=1:6} +
                                          QderivU[1]*((uPrev[1,1]-u_Ol[1,1])^2+sum{(u_Ol[i,1]-u_Ol[i+1,1])^2,i=1:N-delay_a-1})+
                                          QderivU[2]*((uPrev[1,2]-u_Ol[1,2])^2+sum{(u_Ol[i,2]-u_Ol[i+1,2])^2,i=1:N-delay_df-1}))

        # Control Input cost
        # ---------------------------------
        @NLexpression(mdl, controlCost, R[1]*sum{(u_Ol[i,1])^2,i=1:N-delay_a}+
                                        R[2]*sum{(u_Ol[i,2])^2,i=1:N-delay_df})

        # Lane cost (soft)
        # ---------------------------------
        @NLexpression(mdl, laneCost, Q_lane*sum{10.0*eps_lane[i]+100.0*eps_lane[i]^2 ,i=2:N+1})

        # Terminal Cost
        # ---------------------------------
        @NLexpression(mdl, terminalCost , sum{alpha[i]*statesCost[i], i=1:Nl*Np})

        # Slack cost (soft)
        # ---------------------------------
        #@NLexpression(mdl, slackCost, sum{50*eps_alpha[i]+500*eps_alpha[i]^2,i=1:6})

        # Velocity Cost
        #----------------------------------
        #@NLexpression(mdl, velocityCost , Q_vel*sum{10.0*eps_vel[i]+100.0*eps_vel[i]^2 ,i=2:N+1})

        # Path Following Cost
        #----------------------------------

        @NLexpression(mdl, costPF, 0.5*sum{Q[i]*sum{(z_Ol[j,i]-z_Ref[j,i])^2,j=2:N+1},i=1:6})    # Follow trajectory

        # Overall Cost function (objective of the minimization)
        # -----------------------------------------------------

        @NLobjective(mdl, Min, derivCost + laneCost + controlCost + terminalCost )#+ slackCost)#+ velocityCost)
        #@NLobjective(mdl, Min, derivCost + controlCost + costPF + terminalCost)

        sol_stat=solve(mdl)
        println("Finished solve 1: $sol_stat")
        sol_stat=solve(mdl)
        println("Finished solve 2: $sol_stat")

        m.mdl = mdl
        m.z0 = z0
        m.coeff = coeff
        m.z_Ol = z_Ol
        m.u_Ol = u_Ol
        m.c_Vx = c_Vx
        m.c_Vy = c_Vy
        m.c_Psi = c_Psi
        m.uPrev = uPrev
        #m.eps_alpha=eps_alpha

        m.derivCost = derivCost
        m.controlCost = controlCost
        m.costPF = costPF
        #m.laneCost = laneCost
        m.terminalCost= terminalCost # terminal cost
        #m.velocityCost= velocityCost #velocity cost
        m.selStates   = selStates    # selected states
        m.statesCost  = statesCost   # cost of the selected states
        m.alpha       = alpha        # parameters of the convex hull

        return m
    end
end

type MpcModel_obstacle

    mdl::JuMP.Model

    z0::Array{JuMP.NonlinearParameter,1}
    coeff::Array{JuMP.NonlinearParameter,1}
    selStates::Array{JuMP.NonlinearParameter,2}
    statesCost::Array{JuMP.NonlinearParameter,1}
    c_Vx::Array{JuMP.NonlinearParameter,1}
    c_Vy::Array{JuMP.NonlinearParameter,1}
    c_Psi::Array{JuMP.NonlinearParameter,1}
    uPrev::Array{JuMP.NonlinearParameter,2}
    Q_obs::Array{JuMP.NonlinearParameter,1}
    obs::Array{JuMP.NonlinearParameter,2}

    eps_lane::Array{JuMP.Variable,1}
    #eps_vel::Array{JuMP.Variable,1}
    alpha::Array{JuMP.Variable,1}
    z_Ol::Array{JuMP.Variable,2}
    u_Ol::Array{JuMP.Variable,2}

    dsdt::Array{JuMP.NonlinearExpression,1}
    c::Array{JuMP.NonlinearExpression,1}

    derivCost::JuMP.NonlinearExpression
    controlCost::JuMP.NonlinearExpression
    laneCost::JuMP.NonlinearExpression
    terminalCost::JuMP.NonlinearExpression
    slackVx::JuMP.NonlinearExpression
    slackVy::JuMP.NonlinearExpression
    slackPsidot::JuMP.NonlinearExpression
    slackEpsi::JuMP.NonlinearExpression
    slackEy::JuMP.NonlinearExpression
    slackS::JuMP.NonlinearExpression
    obstacleSlackCost::JuMP.NonlinearExpression

    #velocityCost::JuMP.NonlinearExpression

    function MpcModel_obstacle(mpcParams::MpcParams,mpcCoeff::MpcCoeff,modelParams::ModelParams,trackCoeff::TrackCoeff,selectedStates::SelectedStates,obstacle::Obstacle)

        m = new()

        #### Initialize parameters

        dt         = modelParams.dt                # time step
        L_a        = modelParams.l_A               # distance from CoM of the car to the front wheels
        L_b        = modelParams.l_B               # distance from CoM of the car to the rear wheels
        u_lb       = modelParams.u_lb              # lower bounds for the control inputs
        u_ub       = modelParams.u_ub              # upper bounds for the control inputs
        z_lb       = modelParams.z_lb              # lower bounds for the states
        z_ub       = modelParams.z_ub              # upper bounds for the states
        c0         = modelParams.c0

        ey_max      = trackCoeff.width/2           # bound for the state ey (distance from the center track). It is set as half of the width of the track for obvious reasons
        n_poly_curv = trackCoeff.nPolyCurvature    # polynomial degree for curvature approximation
        v_max       = 3                            # maximum allowed velocity

        v_ref      = mpcParams.vPathFollowing              # reference velocity for the path following
        N          = mpcParams.N                           # Prediction horizon
        QderivZ    = mpcParams.QderivZ::Array{Float64,1}   # weights for the derivative cost on the states
        QderivU    = mpcParams.QderivU::Array{Float64,1}   # weights for the derivative cost on the control inputs
        R          = mpcParams.R::Array{Float64,1}         # weights on the control inputs
        Q          = mpcParams.Q::Array{Float64,1}         # weights on the states for path following
        Q_lane     = mpcParams.Q_lane::Float64             # weight on the soft constraint on the lane
        #Q_vel      = mpcParams.Q_vel::Float64              # weight on the soft constraint for the max velocity
        delay_df   = mpcParams.delay_df
        delay_a    = mpcParams.delay_a
        Q_slack    = mpcParams.Q_slack

        r_s        = obstacle.r_s
        r_ey       = obstacle.r_ey

        Np         = selectedStates.Np::Int64              # how many states to select
        Nl         = selectedStates.Nl::Int64              # how many previous laps to select

        acc_f           = 1.0

        mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.09))#,check_derivatives_for_naninf="yes"))#,linear_solver="ma57",print_user_options="yes"))

        @variable( mdl, z_Ol[1:(N+1),1:7])
        @variable( mdl, u_Ol[1:N,1:2])
        @variable( mdl, eps_lane[1:N+1] >= 0)   # eps for soft lane constraints
        @variable( mdl, alpha[1:Nl*Np] >= 0)    # coefficients of the convex hull
       # @variable( mdl, eps_alpha[1:6] >=0)     # eps for soft constraint on alpha
       #@variable( mdl, eps_vel[1:N+1]>=0)      # eps for soft constraint on velocity

        z_lb_6s = ones(mpcParams.N+1,1)*[0.1 -Inf -Inf -Inf -Inf -Inf -Inf]                      # lower bounds on states
        z_ub_6s = ones(mpcParams.N+1,1)*[3.5  Inf Inf  Inf  Inf  Inf Inf]                      # upper bounds
        u_lb_6s = ones(mpcParams.N,1) * [-1.0  -0.3]                                         # lower bounds on steering
        u_ub_6s = ones(mpcParams.N,1) * [2.0   0.3]                                         # upper bounds

        for i=1:2
            for j=1:N
                setlowerbound(u_Ol[j,i], u_lb_6s[j,i])
                setupperbound(u_Ol[j,i], u_ub_6s[j,i])
            end
        end
        for i=1:7
            for j=1:N+1
                setlowerbound(z_Ol[j,i], z_lb_6s[j,i])
                setupperbound(z_Ol[j,i], z_ub_6s[j,i])
            end
        end

        @NLparameter(mdl, z0[i=1:7] == 0)
        @NLparameter(mdl, coeff[i=1:n_poly_curv+1] == 0)
        @NLparameter(mdl, c_Vx[i=1:3]  == 0)
        @NLparameter(mdl, c_Vy[i=1:4]  == 0)
        @NLparameter(mdl, c_Psi[i=1:3] == 0)
        @NLparameter(mdl, uPrev[1:10,1:2] == 0)
        @NLparameter(mdl, selStates[1:Nl*Np,1:6] == 0)                                 # states from the previous trajectories selected in "convhullStates"
        @NLparameter(mdl, statesCost[1:Nl*Np] == 0)                                    # costs of the states selected in "convhullStates"
        @NLparameter(mdl, Q_obs[i=1:Nl*Np] == mpcParams.Q_obs[i])                     # weight used to exclude some states from the convex hull
        @NLparameter(mdl, obs[j=1:N+1,i=1:3] == 0)                                    # nearest obstacle to avoid

        # Conditions for first solve:
        setvalue(z0[1],1)
        setvalue(c_Vx[3],0.1)

        @NLconstraint(mdl, [i=1:7], z_Ol[1,i] == z0[i])

        @NLconstraint(mdl, [i=2:N+1], z_Ol[i,5] <= ey_max + eps_lane[i])
        @NLconstraint(mdl, [i=2:N+1], z_Ol[i,5] >= -ey_max - eps_lane[i])
        #@NLconstraint(mdl,[i = 1:(N+1)], z_Ol[i,4] <= v_max + eps_vel[i] )      # sof constraint on maximum velocity
        @NLconstraint(mdl, sum{alpha[i],i=1:Nl*Np} == 1)                        # constraint on the coefficients of the convex hull

        #for n = 1:6
            #@NLconstraint(mdl,z_Ol[N+1,n] == sum{alpha[j]*selStates[j,n],j=1:Nl*Np})  # terminal constraint
            #@NLconstraint(mdl,z_Ol[N+1,n] >= sum{alpha[j]*selStates[j,n],j=1:Nl*Np}-eps_alpha[n])
            #@NLconstraint(mdl,z_Ol[N+1,n] <= sum{alpha[j]*selStates[j,n],j=1:Nl*Np}+eps_alpha[n])
        #end

        @NLexpression(mdl, c[i = 1:N], sum{coeff[j]*z_Ol[i,6]^(n_poly_curv-j+1),j=1:n_poly_curv} + coeff[n_poly_curv+1])
        @NLexpression(mdl, dsdt[i = 1:N], (z_Ol[i,1]*cos(z_Ol[i,4]) - z_Ol[i,2]*sin(z_Ol[i,4]))/(1-z_Ol[i,5]*c[i]))

        println("Initializing model...")

        # System dynamics
        for i=1:N
            if i<=delay_df
                @NLconstraint(mdl, z_Ol[i+1,2]  == z_Ol[i,2] + c_Vy[1]*z_Ol[i,2]/z_Ol[i,1] + c_Vy[2]*z_Ol[i,1]*z_Ol[i,3] + c_Vy[3]*z_Ol[i,3]/z_Ol[i,1] + c_Vy[4]*uPrev[delay_df+1-i,2]) # yDot
                @NLconstraint(mdl, z_Ol[i+1,3]  == z_Ol[i,3] + c_Psi[1]*z_Ol[i,3]/z_Ol[i,1] + c_Psi[2]*z_Ol[i,2]/z_Ol[i,1] + c_Psi[3]*uPrev[delay_df+1-i,2])                            # psiDot
            else
                @NLconstraint(mdl, z_Ol[i+1,2]  == z_Ol[i,2] + c_Vy[1]*z_Ol[i,2]/z_Ol[i,1] + c_Vy[2]*z_Ol[i,1]*z_Ol[i,3] + c_Vy[3]*z_Ol[i,3]/z_Ol[i,1] + c_Vy[4]*u_Ol[i-delay_df,2]) # yDot
                @NLconstraint(mdl, z_Ol[i+1,3]  == z_Ol[i,3] + c_Psi[1]*z_Ol[i,3]/z_Ol[i,1] + c_Psi[2]*z_Ol[i,2]/z_Ol[i,1] + c_Psi[3]*u_Ol[i-delay_df,2])                            # psiDot
            end
            if i<=delay_a
                #@NLconstraint(mdl, z_Ol[i+1,1]  == z_Ol[i,1] + dt*(uPrev[delay_a+1-i,1] - 0.5*z_Ol[i,1]))
                #@NLconstraint(mdl, z_Ol[i+1,1]  == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2] + c_Vx[2]*z_Ol[i,3] + c_Vx[3]*z_Ol[i,1] + c_Vx[4]*u_Ol[i,1])                              # xDot
                @NLconstraint(mdl, z_Ol[i+1,7]  == z_Ol[i,7] + dt*(uPrev[delay_a+1-i,1]-z_Ol[i,7])*acc_f)
            else
                #@NLconstraint(mdl, z_Ol[i+1,1]  == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2] + c_Vx[2]*z_Ol[i,3] + c_Vx[3]*z_Ol[i,1] + c_Vx[4]*u_Ol[i,1])                              # xDot
                @NLconstraint(mdl, z_Ol[i+1,7]  == z_Ol[i,7] + dt*(u_Ol[i-delay_a,1]-z_Ol[i,7])*acc_f)
            end
            #@NLconstraint(mdl, z_Ol[i+1,1]  == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2] + c_Vx[2]*z_Ol[i,3] + c_Vx[3]*z_Ol[i,1] + c_Vx[4]*z_Ol[i,7])                               # xDot
            #@NLconstraint(mdl, z_Ol[i+1,1]  == z_Ol[i,1] + dt*(z_Ol[i,7] - 0.5*z_Ol[i,1]))                               # xDot
            @NLconstraint(mdl, z_Ol[i+1,1]  == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2]*z_Ol[i,3] + c_Vx[2]*z_Ol[i,1] + c_Vx[3]*z_Ol[i,7])
            @NLconstraint(mdl, z_Ol[i+1,4]  == z_Ol[i,4] + dt*(z_Ol[i,3]-dsdt[i]*c[i]))                                                                                # ePsi
            @NLconstraint(mdl, z_Ol[i+1,5]  == z_Ol[i,5] + dt*(z_Ol[i,1]*sin(z_Ol[i,4])+z_Ol[i,2]*cos(z_Ol[i,4])))                                                      # eY
            @NLconstraint(mdl, z_Ol[i+1,6]  == z_Ol[i,6] + dt*dsdt[i]  )                                                                                # s
        end
        # @NLconstraint(mdl, u_Ol[1,1]-uPrev[1,1] <= 0.05)
        # @NLconstraint(mdl, u_Ol[1,1]-uPrev[1,1] >= -0.2)
        # for i=1:N-1 # Constraints on u:
        #     @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] <= 0.05)
        #     @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] >= -0.2)
        # end

        @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] <= 0.06)
        @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] >= -0.06)
        for i=1:N-1 # Constraints on u:
            @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] <= 0.06)
            @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] >= -0.06)
        end

        # Cost functions

        # Derivative cost
        # ---------------------------------
        @NLexpression(mdl, derivCost, sum{QderivZ[j]*(sum{(z_Ol[i,j]-z_Ol[i+1,j])^2,i=1:N}),j=1:6} +
                                          QderivU[1]*((uPrev[1,1]-u_Ol[1,1])^2+sum{(u_Ol[i,1]-u_Ol[i+1,1])^2,i=1:N-delay_a-1})+
                                          QderivU[2]*((uPrev[1,2]-u_Ol[1,2])^2+sum{(u_Ol[i,2]-u_Ol[i+1,2])^2,i=1:N-delay_df-1}))

        # Control Input cost
        # ---------------------------------
        @NLexpression(mdl, controlCost, R[1]*sum{(u_Ol[i,1])^2,i=1:N-delay_a}+
                                        R[2]*sum{(u_Ol[i,2])^2,i=1:N-delay_df})

        # Lane cost (soft)
        # ---------------------------------
        @NLexpression(mdl, laneCost, Q_lane*sum{10.0*eps_lane[i]+100.0*eps_lane[i]^2 ,i=2:N+1})

        # Terminal Cost
        # ---------------------------------
        @NLexpression(mdl, terminalCost , sum{Q_obs[i]*alpha[i]*statesCost[i], i=1:Nl*Np})

        # Slack cost (soft)
        # ---------------------------------
        #@NLexpression(mdl, slackCost, sum{50*eps_alpha[i]+500*eps_alpha[i]^2,i=1:6})

        # Slack cost on vx
        #----------------------------------
        @NLexpression(mdl, slackVx, (z_Ol[N+1,1] - sum{alpha[j]*selStates[j,1],j=1:Nl*Np})^2)

        # Slack cost on vy
        #----------------------------------
        @NLexpression(mdl, slackVy, (z_Ol[N+1,2] - sum{alpha[j]*selStates[j,2],j=1:Nl*Np})^2)

        # Slack cost on Psi dot
        #----------------------------------
        @NLexpression(mdl, slackPsidot, (z_Ol[N+1,3] - sum{alpha[j]*selStates[j,3],j=1:Nl*Np})^2)

        # Slack cost on ePsi
        #----------------------------------
        @NLexpression(mdl, slackEpsi, (z_Ol[N+1,4] - sum{alpha[j]*selStates[j,4],j=1:Nl*Np})^2)

        # Slack cost on ey
        #----------------------------------
        @NLexpression(mdl, slackEy, (z_Ol[N+1,5] - sum{alpha[j]*selStates[j,5],j=1:Nl*Np})^2)

        # Slack cost on s
        #----------------------------------
        @NLexpression(mdl, slackS, (z_Ol[N+1,6] - sum{alpha[j]*selStates[j,6],j=1:Nl*Np})^2)

        # Soft Constraint on the Obstacle
        # --------------------------------
        @NLexpression(mdl, obstacleSlackCost, 0.1*sum{-log(((z_Ol[i,6]-obs[i,1])/r_s)^2 + ((z_Ol[i,5]-obs[i,2])/r_ey)^2 -1),i=1:N+1})

        # Velocity Cost
        #----------------------------------
        #@NLexpression(mdl, velocityCost , Q_vel*sum{10.0*eps_vel[i]+100.0*eps_vel[i]^2 ,i=2:N+1})

        # Overall Cost function (objective of the minimization)
        # -----------------------------------------------------

        #@NLobjective(mdl, Min, derivCost + laneCost + controlCost + terminalCost )#+ slackCost)#+ velocityCost)

        @NLobjective(mdl, Min, derivCost + laneCost + controlCost + terminalCost + Q_slack[1]*slackVx + Q_slack[2]*slackVy + Q_slack[3]*slackPsidot + Q_slack[4]*slackEpsi + Q_slack[5]*slackEy + Q_slack[6]*slackS + obstacleSlackCost)

        sol_stat=solve(mdl)
        println("Finished solve 1 convhull mpc: $sol_stat")
        sol_stat=solve(mdl)
        println("Finished solve 2 convhull mpc: $sol_stat")

        m.mdl = mdl
        m.z0 = z0
        m.coeff = coeff
        m.z_Ol = z_Ol
        m.u_Ol = u_Ol
        m.c_Vx = c_Vx
        m.c_Vy = c_Vy
        m.c_Psi = c_Psi
        m.uPrev = uPrev
        m.Q_obs       = Q_obs       # weigth to exclude states from optimization problem
        m.obs         = obs         # obstacle to avoid
        #m.eps_alpha=eps_alpha

        m.derivCost = derivCost
        m.controlCost = controlCost
        m.laneCost = laneCost
        m.terminalCost= terminalCost # terminal cost
        #m.velocityCost= velocityCost #velocity cost
        m.selStates   = selStates    # selected states
        m.statesCost  = statesCost   # cost of the selected states
        m.alpha       = alpha        # parameters of the convex hull

        m.obstacleSlackCost=obstacleSlackCost

        m.slackVx     = slackVx
        m.slackVy     = slackVy
        m.slackPsidot = slackPsidot
        m.slackEpsi   = slackEpsi
        m.slackEy     = slackEy
        m.slackS      = slackS

        return m
    end
end

max_stars_repo_name: RoyaFiroozi/barc

max_issues_repo_name: RoyaFiroozi/barc

max_forks_repo_name: RoyaFiroozi/barc


text:
# Convert the original example datasets to a compressed CSV file

# See data/README.md for the source of the input data files
# To regenerate the output file:
# 1) Have all input files ready in the data folder
# 2) Instantiate the package environment for data/src
# 3) Run this script with the root folder of the repository being the working directory

using CSV, CodecZlib, DataFrames, ShiftedArrays, XLSX

function gk()
    var = CSV.read("data/VAR_data.csv", DataFrame)
    iv = CSV.read("data/factor_data.csv", DataFrame)
    # Merge the two datasets
    out = innerjoin(var, iv, on=[:year, :month])
    open(GzipCompressorStream, "data/gk.csv.gz", "w") do stream
        CSV.write(stream, out)
    end
end

function hp()
    df = CSV.read("data/employment.csv", DataFrame, header=[:quarter, :payrolls_a, :payrolls_u])
    open(GzipCompressorStream, "data/hp.csv.gz", "w") do stream
        CSV.write(stream, df)
    end
end

function rz()
    df = DataFrame(XLSX.readtable("data/RZDAT.xlsx", "rzdat", infer_eltypes=true)...)
    df = df[df.quarter.>=1889,:]
    # Generate variables following the replication files
    df.wwii = (df.quarter.>=1941.5) .& (df.quarter.<1946)
    df.slack = df.unemp.>=6.5
    ynorm = df.rgdp_pott6
    df.newsy = df.news./(lag(ynorm).*lag(df.pgdp))
    df.rgov = df.ngov./df.pgdp
    df.y = df.rgdp./ynorm
    df.g = df.rgov./ynorm
    open(GzipCompressorStream, "data/rz.csv.gz", "w") do stream
        CSV.write(stream, df)
    end
end

function bb()
    df = CSV.read("data/data.csv", DataFrame)
    open(GzipCompressorStream, "data/bb.csv.gz", "w") do stream
        CSV.write(stream, df)
    end
end

function main()
    gk()
    hp()
    rz()
    bb()
end

main()


text:
#===============================================================================
# Reversible Jump Change-point Analysis
# July 2018
#
# A relatively straightforward port of Green's Fortran implementation, but
# making use of Hamiltonian Monte Carlo for height updates. (One can also use
# HMC for location updates, but the discontinuities in the energy/likelihood
# function make it less appopriate.)
#
# The simplest way of running this program is to call `rjcp` with the output of
# `readvec`.
===============================================================================#

module RJCP

export green, readvec, rjcp

import Base: show, string

include("structs.jl") #
include("k.jl")       # Must be included
include("loc.jl")     # in this order.
include("ht.jl")      #

#==== Main ====================================================================#

"""
    rjcp(filename; <keyword arguments>)

Performs reversible jump and Hamiltonian MCMC for the Poisson-process
change-point problem.

# Arguments
  - `data::Vector{Float64}`: an array of non-negative event times.
  - `R::Float64=40907.0`: an upper limit on the event times present in the data.
        The interval ``[0,R]`` will be used as an overall time window in the
        likelihood calculations.
  - `k::Union{Int,UnitRange{Int}}=0:20`: allowable values for the number of
        change points. If an integer is given, between-model jumps will not
        occur.
  - `loc::Symbol=:gibbs`, `ht::Symbol=:hmc`: the algorithms to be used for
        location and height updates respectively: `:gibbs` or `:hmc`.
  - `lambda::Float64=3.0`: the rate of the Poisson prior on the number of change
        points.
  - `alpha::Float64=1.0`, `beta::Float64=200.0`: the rates of the Beta prior on
        step heights.
  - `leps::Float64=.005`, `lleaps::Int=5`: leapfrog step size and count for
        location moves (when `loc` is set to `:hmc`).
  - `heps::Float64=.1`, `hleaps::Int=100`: leapfrog step size and count for
        height moves.
  - `samples::Int=40_000`, `burnin::Int=samples/10`: the number of samples to
        draw. Unless otherwise specified, a warm-up or 'burn-in' stage will be
        run prior to the sampling.
  - `frequency::Int=1`: the frequency at which samples are written to file,
        e.g., a frequency of 2 implies that every second sample is discarded.
"""
function rjcp(data::Vector{Float64};
              R::Float64=40907., k::Union{Int,UnitRange{Int}}=0:20,
              loc::Symbol=:gibbs, ht::Symbol=:hmc,
              lambda::Float64=3., alpha::Float64=1., beta::Float64=200.,
              leps::Float64=.005, lleaps::Int=5,
              heps::Float64=.1, hleaps::Int=100,
              samples::Int=40_000, burnin::Int=div(samples,10),
              frequency::Int=1)

    kmin,kmax = (k isa Int) ? (k,k) : extrema(k)
    Y = sort(data)
    C = Constants(kmax,R,lambda,alpha,beta,leps,lleaps,heps,hleaps)
    S = State(Y,kmin,kmax,R)

    X = Auxiliary(kmax)
    aux_init(Y,C,S,X)

    J = Jumps(k -> min(1,poissonratio(lambda,k)),
              k -> min(1,rpoissonratio(lambda,k-1)), kmin, kmax, .9)

    function rjmcmc(name::AbstractString, I::Int)
        (I <= 0) && return

        U = Counts(kmax)
        open(name*".txt", "w") do f
            write(f, header(kmax)*"\n")
            for i = 1:I
                if !isa(k,Int) rjmove(Y,C,S,X,J,U) end
                locmove(loc,Y,C,S,X,U)
                htmove(ht,Y,C,S,X,U)
                U.ks[S.k+1] += 1
                if (i % frequency == 0) write(f, string(S)*"\n") end
            end
        end
        println(U)
    end

    rjmcmc("burnin", burnin)
    rjmcmc("samples", samples)

end

function aux_init(Y,C,S,X)
    loc_init(Y,S,X)
    loc_unconstrained(S.k,C,X)
    ht_init(Y,S,X)
    ht_unconstrained(S.k,X)
end

#==== I/O =====================================================================#

"""
    readvec(path)

Returns a `Vector{Float64}` of the numbers contained in the given file.

# Arguments
  - `path::Union{AbstractString,IO}`: the name or `IO` instance of the file
        containing the change-point data. These should be non-negative numbers
        formatted in a single row, column, or a matrix with no missing elements.
        Columns should be comma-separated.
"""
function readvec(path::Union{AbstractString,IO})
    vec(readdlm(path, ',', Float64))
end

"""
    green(name)

Converts output from Green's change-point program to the format of this program.

# Arguments
  - `name::AbstractString`: the shared of the two input files (`name.pos` and
        `name.ht`).
"""
function green(name::AbstractString)

    open(name*".pos") do pos
        open(name*".ht") do ht
            open(name*".txt", "w") do out

                values(l) = map(s -> parse(Float64,s), split(l))

                ps = [values(l) for l in readlines(pos)]
                hs = [values(l) for l in readlines(ht)]
                K = mapreduce(length,max,0,ps)
                S = State(readvec("coal.csv"),1,K,40907.)

                write(out, header(K)*"\n")
                for (i,p) in enumerate(ps)
                    k = length(p)
                    S.k = k
                    S.locs[1:k+2] .= vcat(0.,p,40907.)
                    S.hts[1:k+1] .= hs[i]
                    write(out, string(S)*"\n")
                end

            end
        end
    end

end

end

max_stars_repo_name: kevdur/rjcp

max_issues_repo_name: kevdur/rjcp

max_forks_repo_name: kevdur/rjcp


text:
using Documenter, Dualization

makedocs(
    modules = [Dualization],
    doctest  = false,
    clean = true,
    # See https://github.com/JuliaDocs/Documenter.jl/issues/868
    format = Documenter.HTML(assets = ["assets/favicon.ico"],
                    mathengine = Documenter.MathJax(),
                    prettyurls = get(ENV, "CI", nothing) == "true"),
    sitename = "Dualization.jl",
    authors = "Guilherme Bodin, and contributors",
    pages = [
        "Home" => "index.md",
        "manual.md",
        "examples.md",
        "reference.md"
    ]
)

deploydocs(
    repo = "github.com/jump-dev/Dualization.jl.git",
)


text:
using Latexify

@latexrecipe function f(sym::LogicalSymbol)
    return name(sym)
end

@latexrecipe function f(op::LogicalOperation)
    return name(op)
end

@latexrecipe function f(expr::LogicalExpression)
    env --> :eq

    opname = name(operation(expr))
    if isunary(operation(expr))
        return "$(opname)($(first(arguments(expr))))"
    end
    if isbinary(operation(expr))
        return "$(arguments(expr)[1]) $(opname) $(arguments(expr)[2])"
    end
end


text:
using WENO4
using Documenter

DocMeta.setdocmeta!(WENO4, :DocTestSetup, :(using WENO4); recursive=true)

makedocs(;
    modules=[WENO4],
    authors="Tiago M. D. Pereira",
    repo="https://github.com/tiagopereira/WENO4.jl/blob/{commit}{path}#{line}",
    sitename="WENO4.jl",
    format=Documenter.HTML(;
        prettyurls=get(ENV, "CI", "false") == "true",
        canonical="https://tiagopereira.github.io/WENO4.jl",
        assets=String[],
    ),
    pages=[
        "Home" => "index.md",
    ],
)

deploydocs(;
    repo="github.com/tiagopereira/WENO4.jl",
    devbranch="main",
)


text:
abstract type CovKer end

"""
    SqExp(σ)

Stores the bandwidth parameter `σ` of the squared exponential covariance kernel.

See also: [`(s_e::SqExp)(x, y)`](@ref)
"""
struct SqExp{F <: Real} <: CovKer
    neg_inv_double_σ_sq::F

    SqExp(σ::F = 1.0) where F <: Real = new{F}(-F(0.5)/(σ^2))
end

"""
    (s_e::SqExp)(x, y)

Evaluates the squared exponential covariance kernel at `(x, y)`.
"""
(s_e::SqExp)(x::V1, y::V2) where {V1 <: AbstractVector, V2 <: AbstractVector} =
    exp(dot((x - y),(x - y))*s_e.neg_inv_double_σ_sq)

"""
    (s_e::SqExp)(X)

Evaluates the s_e for all `(X_i, X_j)`.
"""

function (s_e::SqExp)(x_samples::AbstractMatrix{F}) where {F<:Real}
    num_samples = size(x_samples, 2)
    K = Array{F, 2}(undef, num_samples, num_samples)
    for i in 1:num_samples
        for j in i:num_samples
            K[i, j] = s_e(x_samples[:,i], x_samples[:,j])
        end
    end
    K
end


text:
function solve()
    d, t, s = [parse(Int, x) for x in split(readline())]
    s*t >= d ? "Yes" : "No"
end

println(solve())

max_stars_repo_name: aishikawa/atcoder-julia

max_issues_repo_name: aishikawa/atcoder-julia

max_forks_repo_name: aishikawa/atcoder-julia


text:
using LocalitySensitive
using BenchmarkTools

benchmark_documents = readlines("../resources/benchmark_data.csv")

function fingerprint_benchmark(docs::Vector{String}, mh::MinHash)
    for d in docs
       fingerprint(mh, shingle(d))
    end
end

@info "Benchmark shingling and fingerprinting"
mh = MinHash()
@benchmark fingerprint_benchmark(benchmark_documents, mh)
shingles = [shingle(d) for d in benchmark_documents]
@benchmark fingerprint_all(mh, shingles)

@info "Benchmark push to index"
fingerprints = [fingerprint(mh, shingle(d)) for d in benchmark_documents]
mhind = MinHashIndex(minhash=mh)
function pushall(fingerprints, mhind)
    for f in fingerprints
        push!(mhind, f)
    end
end
@benchmark pushall(fingerprints, mhind)

@info "Benchmark all similar pairs"
mhind = MinHashIndex(minhash=mh)
pushall(fingerprints, mhind)
@benchmark similar_pairs(mhind)

pairs = similar_pairs(mhind)


text:
using OpenCL

const bench_kernel = "
__kernel void sum(__global const float *a,
                  __global const float *b,
                  __global float *c)
{
        int gid = get_global_id(0);
        float a_temp;
        float b_temp;
        float c_temp;

        a_temp = a[gid]; // my a element (by global ref)
        b_temp = b[gid]; // my b element (by global ref)

        c_temp = a_temp+b_temp; // sum of my elements
        c_temp = c_temp * c_temp; // product of sums
        c_temp = c_temp * (a_temp/2.0f); // times 1/2 my a

        c[gid] = c_temp; // store result in global memory
}"

function cl_performance(ndatapts::Integer, nworkers::Integer)

    @assert ndatapts > 0
    @assert nworkers > 0

    a = rand(Float32,  ndatapts)
    b = rand(Float32,  ndatapts)
    c = Vector{Float32}(ndatapts)

    @printf("Size of test data: %i MB\n", sizeof(a) / 1024 / 1024)

    t1 = time()
    for i in 1:ndatapts
        c_temp = a[i] + b[i]
        c_temp = c_temp * c_temp
        c[i]   = c_temp * (a[i] / 2f0)
    end
    t2 = time()

    @printf("Julia Execution time: %.4f seconds\n", t2 - t1)

    for platform in cl.platforms()

        if platform[:name] == "Portable Computing Language"
            warn("Portable Computing Language platform not yet supported")
            continue
        end

        for device in cl.available_devices(platform)
            @printf("====================================================\n")
            @printf("Platform name:    %s\n",  platform[:name])
            @printf("Platform profile: %s\n",  platform[:profile])
            @printf("Platform vendor:  %s\n",  platform[:vendor])
            @printf("Platform version: %s\n",  platform[:version])
            @printf("----------------------------------------------------\n")
            @printf("Device name: %s\n", device[:name])
            @printf("Device type: %s\n", device[:device_type])
            @printf("Device mem: %i MB\n",           device[:global_mem_size] / 1024^2)
            @printf("Device max mem alloc: %i MB\n", device[:max_mem_alloc_size] / 1024^2)
            @printf("Device max clock freq: %i MHZ\n",  device[:max_clock_frequency])
            @printf("Device max compute units: %i\n",   device[:max_compute_units])
            @printf("Device max work group size: %i\n", device[:max_work_group_size])
            @printf("Device max work item size: %s\n",  device[:max_work_item_size])

            if device[:max_mem_alloc_size] < sizeof(Float32) * ndatapts
                warn("Requested buffer size exceeds device max alloc size!")
                warn("Skipping device $(device[:name])...")
                continue
            end

            if device[:max_work_group_size] < nworkers
                warn("Number of workers exceeds the device's max work group size!")
                warn("Skipping device $(device[:name])...")
                continue
            end

            ctx   = cl.Context(device)
            queue = cl.CmdQueue(ctx, :profile)

            a_buf = cl.Buffer(Float32, ctx, (:r, :copy), hostbuf=a)
            b_buf = cl.Buffer(Float32, ctx, (:r, :copy), hostbuf=b)
            c_buf = cl.Buffer(Float32, ctx, :w, length(a))

            prg  = cl.Program(ctx, source=bench_kernel) |> cl.build!
            kern = cl.Kernel(prg, "sum")

            # work_group_multiple = kern[:prefered_work_group_size_multiple]
            global_size = (ndatapts,)
            local_size  = (nworkers,)

            # call the kernel
            evt = kern[queue, global_size, local_size](a_buf, b_buf, c_buf)

            # duration in ns
            t = evt[:profile_duration] * 1e-9
            @printf("Execution time of test: %.4f seconds\n", t)

            c_device = cl.read(queue, c_buf)
            info("Result norm: $(norm(c - c_device))")
        end
    end
end

# Play with these numbers to see performance differences
# N_DATAPTS has to be a multiple of the number of workers
# N_WORKERS has to be less than or equal to the device's max work group size
# ex. N_WORKERS = 1 is non parallel execution on the gpu

const N_DATA_PTS = Int(2^23) # ~8 million
const N_WORKERS  = Int(2^7)
cl_performance(N_DATA_PTS, N_WORKERS)