text:
# --------------------------------------------------------------------------
# ACE.jl and SHIPs.jl: Julia implementation of the Atomic Cluster Expansion
# Copyright (c) 2019 Christoph Ortner <christophortner0@gmail.com>
# Licensed under ASL - see ASL.md for terms and conditions.
# --------------------------------------------------------------------------

import ACE.DAG: CorrEvalGraph, get_eval_graph, traverse_fwd!

export PIBasis

export graphevaluator, standardevaluator

"""
`PIBasisFcn{N, TOP}` : represents a single multivariate basis function
in terms of 1-particle pasis functions in each coordinate direction. Crucially,
this function will be interpreted as a *permutation invariant* basis function!
"""
struct PIBasisFcn{N, TOP <: OnepBasisFcn}
   z0::AtomicNumber
   oneps::NTuple{N, TOP}
   top::Type{TOP}
end

_top(::PIBasisFcn{N, TOP}) where {N, TOP} = TOP

PIBasisFcn(z0::AtomicNumber, oneps) =
   PIBasisFcn(z0, tuple(oneps...), typeof(oneps[1]))

order(b::PIBasisFcn{N}) where {N} = N

degree(d::AbstractDegree, pphi::PIBasisFcn) = degree(d, pphi.oneps)

# TODO: this is very rough - can we do better?
scaling(b::PIBasisFcn, p) = sum(scaling(bb, p) for bb in b.oneps)

# TODO: can we replace this with get_basis_spec?
function PIBasisFcn(Aspec, t, z0::AtomicNumber)
   if isempty(t) || sum(abs, t) == 0
      TOP = eltype(Aspec)
      return PIBasisFcn{0, eltype(Aspec)}(z0, NTuple{0, TOP}(), TOP)
   end
   # zeros stand for reduction in body-order
   tnz = t[findall(t .!= 0)]
   return PIBasisFcn(z0, Aspec[[tnz...]])
end

write_dict(b::PIBasisFcn) =
   Dict("__id__" => "ACE_PIBasisFcn",
        "z0" => write_dict(b.z0),
        "oneps" => write_dict.(b.oneps))

read_dict(::Val{:SHIPs_PIBasisFcn}, D::Dict) =
   read_dict(Val{:ACE_PIBasisFcn}(), D::Dict)

read_dict(::Val{:ACE_PIBasisFcn}, D::Dict) =
   PIBasisFcn( read_dict(D["z0"]),
               tuple( read_dict.(D["oneps"]) ... ) )

"""
note this function doesn't return an ordered specification, this is due
to the fact that we don't require the Aspec to be ordered by degree.
Instead the ordering is achieved in the InnerPIBasis constructor
"""
function get_PI_spec(basis1p::OneParticleBasis, N::Integer,
                     degree::AbstractDegree, maxdeg::Real,
                     z0::AtomicNumber; filter = _->true )
   iz0 = z2i(basis1p, z0)
   # get the basis spec of the one-particle basis
   #  Aspec[i] described the basis function that will get written into A[i]
   #  but we don't care here since we will just map back and forth in the
   #  pre-computation stage. note AAspec below will not store indices to Aspec
   #  but the actual basis functions themselves.
   Aspec = get_basis_spec(basis1p, z0)
   # next we need to sort it by degree so that gensparse doesn't get confused.
   Aspec_p = sort(Aspec, by = b -> degree(b, z0))
   # now an index νi corresponds to the basis function
   # Aspec[p[νi]] = Aspec_p[νi] and a tuple ν = (ν1,...,νN) to the following
   # basis function
   tup2b = ν -> PIBasisFcn(Aspec_p, ν, z0)
   # we can now construct the basis specification; the `ordered = true`
   # keyword signifies that this is a permutation-invariant basis
   AAspec = gensparse(N, maxdeg;
                      tup2b = tup2b, degfun = degree, ordered = true,
                      maxν = length(Aspec_p),
                      filter = filter)
   return AAspec
end

"""
`mutable struct InnerPIBasis` : this type is just an auxilary type to
make the implementation of `PIBasis` clearer. It implements the
permutation-invariant basis for a single centre-atom species. The main
type `PIBasis` then stores `NZ` objects of type `InnerPIBasis`
and "dispatches" the work accordingly.
"""
mutable struct InnerPIBasis
   orders::Vector{Int}           # order (length) of ith basis function
   iAA2iA::Matrix{Int}           # where in A can we find the ith basis function
   b2iAA::Dict{PIBasisFcn, Int}  # mapping PIBasisFcn -> iAA =  index in AA[z0]
   b2iA::Dict{Any, Int}          # mapping from 1-p basis fcn to index in A[z0]
   AAindices::UnitRange{Int}     # where in AA does AA[z0] fit?
   z0::AtomicNumber              # inner basis for which species?
   dag::CorrEvalGraph{Int, Int}  # for fast evaluation
end

==(B1::InnerPIBasis, B2::InnerPIBasis) = (
   (B1.b2iA == B2.b2iA) &&
   (B1.iAA2iA == B2.iAA2iA) )

maxorder(basis::InnerPIBasis) = size(basis.iAA2iA, 2)

Base.length(basis::InnerPIBasis) = length(basis.orders)

function InnerPIBasis(Aspec, AAspec, AAindices, z0)
   len = length(AAspec)
   maxorder = maximum(order, AAspec)

   # construct the b2iA mapping
   b2iA = Dict{Any, Int}()
   for (iA, b) in enumerate(Aspec)
      if haskey(b2iA, b)
         @show b
         error("b2iA already has the key b. This means the basis spec is invalid.")
      end
      b2iA[b] = iA
   end

   # allocate the two main arrays used for evaluation ...
   orders = zeros(Int, len)
   iAA2iA = zeros(Int, len, maxorder)
   # ... and fill them up with the cross-indices
   for (iAA, b) in enumerate(AAspec)
      @assert b.z0 == z0
      b = _get_ordered(b2iA, b)
      AAspec[iAA] = b

      orders[iAA] = order(b)
      for α = 1:order(b)
         iAA2iA[iAA, α] = b2iA[ b.oneps[α] ]
      end
   end

   # now sort iAA2iA lexicographically by rows to make sure the representation
   perm = sortperm( [ vcat([orders[iAA]], iAA2iA[iAA,:]) for iAA = 1:len ] )
   iAA2iA = iAA2iA[perm, :]
   AAspec = AAspec[perm]
   orders = orders[perm]

   # construct the b2iAA mapping
   b2iAA = Dict{PIBasisFcn, Int}()
   for (iAA, b) in enumerate(AAspec)
      if haskey(b2iAA, b)
         @show b
         error("b2iAA already has the key b. This means the basis spec is invalid.")
      end
      b2iAA[b] = iAA
   end

   # putting it all together :
   # an empty dag, to assemble the basis ...
   emptydag = DAG.CorrEvalGraph{Int, Int}()
   inner = InnerPIBasis(orders, iAA2iA, b2iAA, b2iA, AAindices, z0, emptydag)
   # ... generate the actual dag to return
   generate_dag!(inner)
   return inner
end

function generate_dag!(inner; kwargs...)
   len = length(inner)
   inner.dag = get_eval_graph(inner, collect(1:len); kwargs...)
   return inner
end

# ---------------------- PIBasis

struct DAGEvaluator end
struct StandardEvaluator end

"""
`mutable struct PIBasis:` implementation of a permutation-invariant
basis based on the density projection trick.

The standard constructor is
```
PIBasis(basis1p, N, D, maxdeg)
```
* `basis1p` : a one-particle basis
* `N` : maximum interaction order
* `D` : an abstract degee specification, e.g., SparsePSHDegree
* `maxdeg` : the maximum polynomial degree as measured by `D`

Note the species list will be taken from `basis1p`
"""
mutable struct PIBasis{BOP, NZ, TEV} <: IPBasis
   basis1p::BOP             # a one-particle basis
   zlist::SZList{NZ}
   inner::NTuple{NZ, InnerPIBasis}
   evaluator::TEV
end

cutoff(basis::PIBasis) = cutoff(basis.basis1p)

==(B1::PIBasis, B2::PIBasis) = ACE._allfieldsequal(B1, B2)

fltype(basis::PIBasis) = fltype(basis.basis1p)

Base.length(basis::PIBasis) = sum(length(basis, iz) for iz = 1:numz(basis))
Base.length(basis::PIBasis, iz0::Integer) = length(basis.inner[iz0])
Base.length(basis::PIBasis, z0::AtomicNumber) = length(basis, z2i(basis, z0))

function PIBasis(basis1p::OneParticleBasis,
                 N::Integer,
                 D::AbstractDegree,
                 maxdeg::Real;
                 filter = b -> order(b) > 0)
   innerspecs = Any[]
   # now for each iz0 (i.e. for each z0) construct an "inner basis".
   for iz0 = 1:numz(basis1p)
      z0 = i2z(basis1p, iz0)
      # get a list of 1-p basis function
      AAspec_z0 = get_PI_spec(basis1p, N, D, maxdeg, z0; filter=filter)
      push!(innerspecs, AAspec_z0)
   end
   return pibasis_from_specs(basis1p, identity.(innerspecs))
end

# TODO: instead of copying zlist, maybe forward the zlist methods

function pibasis_from_specs(basis1p, innerspecs)
   idx = 0
   inner = InnerPIBasis[]
   for iz0 = 1:numz(basis1p)
      z0 = i2z(basis1p, iz0)
      Aspec_z0 = get_basis_spec(basis1p, z0)
      AAspec_z0 = innerspecs[iz0]
      AAindices = (idx+1):(idx+length(AAspec_z0))
      push!(inner, InnerPIBasis(Aspec_z0, AAspec_z0, AAindices, z0))
      idx += length(AAspec_z0)
   end
   return PIBasis(basis1p, zlist(basis1p), tuple(inner...), DAGEvaluator())
end

"""
`get_basis_spec(basis::PIBasis, iz0::Integer, i::Integer)`

Here `i` is the index of the basis function for which we reconstruct its
specification.
"""
function get_basis_spec(basis::PIBasis, iz0::Integer, i::Integer)
   N = basis.inner[iz0].orders[i]
   iAA2iA = basis.inner[iz0].iAA2iA[i, 1:N]
   if N == 0
      # TODO - Nasty hack; I'm assuming all TOPs are the same
      TOP = typeof(get_basis_spec(basis.basis1p, iz0, 1))
      return PIBasisFcn(i2z(basis, iz0), NTuple{0, TOP}(), TOP)
   end
   return PIBasisFcn( i2z(basis, iz0),
                  [ get_basis_spec(basis.basis1p, iz0, iAA2iA[n]) for n = 1:N] )
end

function _get_ordered(pibasis::PIBasis, pib::PIBasisFcn)
   inner = pibasis.inner[z2i(pibasis, pib.z0)]
   return _get_ordered(inner.b2iA, pib)
end

_get_ordered(::Dict, pib::PIBasisFcn{0}) = pib
_get_ordered(::Dict, pib::PIBasisFcn{1}) = pib

function _get_ordered(b2iA::Dict, pib::PIBasisFcn{N}) where {N}
   iAs = [ b2iA[b] for b in pib.oneps ]
   p = sortperm(iAs)
   return PIBasisFcn(pib.z0, ntuple(i -> pib.oneps[p[i]], N))
end

function scaling(pibasis::PIBasis, p)
   ww = zeros(Float64, length(pibasis))
   for iz0 = 1:numz(pibasis)
      wwin = @view ww[pibasis.inner[iz0].AAindices]
      for i = 1:length(pibasis.inner[iz0])
         bspec = get_basis_spec(pibasis, iz0, i)
         wwin[i] = scaling(bspec, p)
      end
   end
   return ww
end

graphevaluator(basis::PIBasis) =
   PIBasis(basis.basis1p, zlist(basis), basis.inner, DAGEvaluator())

standardevaluator(basis::PIBasis) =
   PIBasis(basis.basis1p, zlist(basis), basis.inner, StandardEvaluator())

maxorder(basis::PIBasis) = maximum(maxorder.(basis.inner))

# -------------------------------------------------
# FIO codes

# TODO: at the moment the DAGs will be generated from scratch
#       every time we load the basis...

write_dict(basis::PIBasis) =
   Dict(  "__id__" => "ACE_PIBasis",
         "basis1p" => write_dict(basis.basis1p),
           "inner" => [ write_dict.( collect(keys(basis.inner[iz0].b2iAA)) )
                         for iz0 = 1:numz(basis) ], )

read_dict(::Val{:SHIPs_PIBasis}, D::Dict) =
   read_dict(Val{:ACE_PIBasis}(), D::Dict)

function read_dict(::Val{:ACE_PIBasis}, D::Dict)
   basis1p = read_dict(D["basis1p"])
   innerspecs = [ read_dict.(D["inner"][iz0])  for iz0 = 1:numz(basis1p) ]
   return pibasis_from_specs(basis1p, innerspecs)
end

# -------------------------------------------------
# Evaluation codes

site_alloc_B(basis::PIBasis, args...) =
      zeros( fltype(basis), maximum(length.(basis.inner)) )

alloc_temp(basis::PIBasis, args...) =
      ( A = alloc_B(basis.basis1p, args...),
        tmp_basis1p = alloc_temp(basis.basis1p, args...),
        tmp_inner = alloc_temp(basis.evaluator, basis)
      )

evaluate!(AA, tmp, basis::PIBasis, args...) =
   evaluate!(AA, tmp, basis::PIBasis, basis.evaluator, args...)

# this method treats basis as a actual basis across all species
function evaluate!(AA, tmp, basis::PIBasis, Rs, Zs, z0)
   fill!(AA, 0)
   iz0 = z2i(basis, z0)
   AAview = @view AA[basis.inner[iz0].AAindices]
   site_evaluate!(AAview, tmp, basis, Rs, Zs, z0)
   return AA
end

# this method treats basis as a basis for a single species z0, i.e.
# AA is assumed to only contain the AA^{z0}.
function site_evaluate!(AA, tmp, basis::PIBasis, Rs, Zs, z0)
   # compute the (multi-variate) density projection
   A = evaluate!(tmp.A, tmp.tmp_basis1p, basis.basis1p, Rs, Zs, z0)
   # now evaluate the correct inner basis, which doesn't know about z0, but
   # only ever sees A
   iz0 = z2i(basis, z0)
   evaluate!(AA, tmp, basis.inner[iz0], basis.evaluator, A)
   return @view(AA[1:length(basis.inner[iz0])])
end

# gradients

site_alloc_dB(basis::PIBasis, Rs::AbstractVector, args...) =
   site_alloc_dB(basis, length(Rs))

site_alloc_dB(basis::PIBasis, nmax::Integer) =
      zeros( JVec{fltype(basis)}, maximum(length.(basis.inner)), nmax )

alloc_temp_d(basis::PIBasis, Rs::AbstractVector, args...) =
   alloc_temp_d(basis, length(Rs))

alloc_temp_d(basis::PIBasis, nmax::Integer) =
      (
        A = alloc_B(basis.basis1p, nmax),
        dA = alloc_dB(basis.basis1p, nmax),
        tmp_basis1p = alloc_temp(basis.basis1p, nmax),
        tmpd_basis1p = alloc_temp_d(basis.basis1p, nmax),
        tmpd_inner = alloc_temp_d(basis.evaluator, basis, nmax)
      )

function evaluate_d!(AA, dAA, tmpd, basis::PIBasis, Rs, Zs, z0)
   iz0 = z2i(basis, z0)
   AAview = @view AA[basis.inner[iz0].AAindices]
   dAAview = @view dAA[basis.inner[iz0].AAindices, 1:length(Rs)]
   site_evaluate_d!(AAview, dAAview, tmpd, basis, Rs, Zs, z0)
   return dAA
end

function site_evaluate_d!(AA, dAA, tmpd, basis::PIBasis, Rs, Zs, z0)
   iz0 = z2i(basis, z0)
   # precompute the 1-p basis and its derivatives
   evaluate_d!(tmpd.A, tmpd.dA, tmpd.tmpd_basis1p, basis.basis1p, Rs, Zs, z0)
   site_evaluate_d!(AA, dAA, tmpd, basis.inner[iz0], basis.evaluator, tmpd.A, tmpd.dA)
   return nothing
end

#--------------------------------------------------------
# ------- specialised code for the standard evaluator

alloc_temp(::StandardEvaluator, basis::PIBasis) = nothing

alloc_temp_d(::StandardEvaluator, basis::PIBasis, args...) = nothing

# this method evaluates the InnerPIBasis, which is really the actual
# evaluation code; the rest is just bookkeeping
function evaluate!(AA, tmp, basis::InnerPIBasis, ::StandardEvaluator, A)
   fill!(AA, 1)
   for i = 1:length(basis)
      for α = 1:basis.orders[i]
         iA = basis.iAA2iA[i, α]
         AA[i] *= A[iA]
      end
   end
   return AA
end

function site_evaluate_d!(AA, dAA, tmpd, inner::InnerPIBasis,
                          _ev::StandardEvaluator, A, dA)
   # evaluate the AA basis
   evaluate!(AA, nothing, inner, _ev, tmpd.A)
   # loop over all neighbours
   for j = 1:size(dA, 2)
      # write the gradients into the correct slice of the dAA matrix
      for iAA = 1:length(inner)
         @inbounds dAA[iAA, j] = grad_AAi_Rj(iAA, j, inner, A, dA)
      end
   end
   return nothing
end

"""
Compute ∂∏A_a / ∂A_b = ∏_{a ≂̸ b} A_a
"""
function grad_AAi_Ab(iAA, b, inner, A)
   g = one(eltype(A))
   @inbounds for a = 1:inner.orders[iAA]
      if a != b
         g *= A[inner.iAA2iA[iAA, a]]
      end
   end
   return g
end

@doc raw"""
Compuate ∂∏A_a / ∂Rⱼ:
```math
   \frac{\partial \prod_a A_a}{\partial \bm r_j}
   =
   \sum_b \frac{\prod_{a} A_a}{\partial A_b} \cdot \frac{\partial A_b}{\partial \bm r_j}
   =
   \sum_b \prod_{a \neq b} A_a \cdot \frac{\partial \phi_b}{\partial \bm r_j}
```
"""
function grad_AAi_Rj(iAA, j, inner, A, dA)
   g = zero(eltype(dA))
   @inbounds for b = 1:inner.orders[iAA] # interaction order
      # A_{k_b} = A[iA]
      iA = inner.iAA2iA[iAA, b]
      # dAAi_dAb = ∂(∏A_{n_a}) / ∂A_{n_b}
      dAAi_dAb = grad_AAi_Ab(iAA, b, inner, A)
      g += dAAi_dAb * dA[iA, j]
   end
   return g
end

# --------------------------------------------------------
#   Evaluation code for DAG evaluator

alloc_temp(::DAGEvaluator, basis::PIBasis) =
   zeros(fltype(basis), maximum(inner.dag.numstore for inner in basis.inner))

function alloc_temp_d(::DAGEvaluator, basis::PIBasis,  nmax::Integer)
   maxstore = maximum(inner.dag.numstore for inner in basis.inner)
   return (
      AA = zeros(fltype(basis), maxstore),
      dAA = zeros(JVec{fltype(basis)}, maxstore, nmax)
   )
end

function evaluate!(AA, tmp, basis::InnerPIBasis, ::DAGEvaluator, A)
   AAdag = tmp.tmp_inner
   fill!(AAdag, 1)
   traverse_fwd!(AAdag, basis.dag, A,
                 (idx, AAval) -> if idx > 0; AA[idx] = AAval; end)
   return AA
end

function site_evaluate_d!(AA, dAA, tmpd, inner::InnerPIBasis,
                          ::DAGEvaluator, A, dA)
   dag = inner.dag
   nodes = dag.nodes
   idxs = dag.vals
   AAtmp = tmpd.tmpd_inner.AA
   dAAtmp = tmpd.tmpd_inner.dAA
   numneigs = size(dA, 2)
   # --- manual bounds checking
   @assert size(dAA, 2) >= numneigs
   @assert size(dAAtmp, 2) >= numneigs
   @assert length(AAtmp) >= inner.dag.numstore
   @assert size(dAAtmp, 1) >= inner.dag.numstore
   # ------------------

   @inline function _copyrow!(A_, iA, B_, iB)
      @simd for j = 1:numneigs
         @inbounds A_[iA,j] = B_[iB,j]
      end
   end

   @inline function _fwdgrad!(dAA_, i, n1, AA1, n2, AA2)
      @simd for j = 1:numneigs
         @inbounds dAA_[i, j] = AA1 * dAAtmp[n2,j] + AA2 * dAAtmp[n1,j]
      end
   end

   @inbounds for i = 1:dag.num1
      idx = idxs[i]
      AAtmp[i] = A[i]
      _copyrow!(dAAtmp, i, dA, i)
      if idx > 0
         AA[idx] = AAtmp[i]
         _copyrow!(dAA, idx, dAAtmp, i)
         # @. dAA[idx,:] = (@view dAAtmp[i,:])
      end
   end

   @inbounds for i = (dag.num1+1):dag.numstore
      n1, n2 = nodes[i]
      idx = idxs[i]
      AA1, AA2 = AAtmp[n1], AAtmp[n2]
      AAtmp[i] = AA1 * AA2
      _fwdgrad!(dAAtmp, i, n1, AA1, n2, AA2)
      # @. dAAtmp[i, :] = AA1 * (@view dAAtmp[n2,:]) + AA2 * (@view dAAtmp[n1,:])
      if idx > 0
         AA[idx] = AAtmp[i]
         _copyrow!(dAA, idx, dAAtmp, i)
         # @. dAA[idx, :] .= (@view dAAtmp[i, :])
      end
   end

   @inbounds for i = (dag.numstore+1):length(dag)
      idx = idxs[i]
      n1, n2 = nodes[i]
      # @assert idx > 0
      AA1, AA2 = AAtmp[n1], AAtmp[n2]
      AA[idx] = AA1 * AA2
      _fwdgrad!(dAA, idx, n1, AA1, n2, AA2)
      # @simd for j = 1:numneigs
      #    @inbounds dAA[idx, j] = AA1 * dAAtmp[n2,j] + AA2 * dAAtmp[n1,j]
      # end
   end

   return nothing
end


text:
using EasyTranspose
using Test

@test [1, 2, 3]ᵀ == [1 2 3]

ext: jl


text:
module testUtils_states
using Test
using LinearAlgebra

srcDir = dirname(dirname(@__FILE__))*"\\src\\"

include(srcDir*"Examples\\Utils_states.jl")
using .Utils_states

@testset "take_Pᵀ" begin
    d = Dict()
    d[1] = (2,2)
    d[2] = (2,3)
    d[3] = (3,3)
    d[4] = (3,4)

    ρ_test = Dict()
    ρ_test[1] =  [1  2   9  10
                  5  6  13  14
                  3  4  11  12
                  7  8  15  16]

    ρ_test[2] =     [ 1   2  13  14  25  26
                      5   6  17  18  29  30
                      9  10  21  22  33  34
                      3   4  15  16  27  28
                      7   8  19  20  31  32
                     11  12  23  24  35  36]

    ρ_test[3] =    [ 1   2   3  28  29  30  55  56  57
                    10  11  12  37  38  39  64  65  66
                    19  20  21  46  47  48  73  74  75
                    4   5   6  31  32  33  58  59  60
                    13  14  15  40  41  42  67  68  69
                    22  23  24  49  50  51  76  77  78
                    7   8   9  34  35  36  61  62  63
                    16  17  18  43  44  45  70  71  72
                    25  26  27  52  53  54  79  80  81]

    ρ_test[4] =     [1   2   3  37  38  39   73   74   75  109  110  111
                     10  11  12  46  47  48   82   83   84  118  119  120
                     19  20  21  55  56  57   91   92   93  127  128  129
                     28  29  30  64  65  66  100  101  102  136  137  138
                      4   5   6  40  41  42   76   77   78  112  113  114
                     13  14  15  49  50  51   85   86   87  121  122  123
                     22  23  24  58  59  60   94   95   96  130  131  132
                     31  32  33  67  68  69  103  104  105  139  140  141
                      7   8   9  43  44  45   79   80   81  115  116  117
                     16  17  18  52  53  54   88   89   90  124  125  126
                     25  26  27  61  62  63   97   98   99  133  134  135
                     34  35  36  70  71  72  106  107  108  142  143  144]

    gen_ρ(d) =  reshape(1:(d[1]*d[2])^2,d[1]^2,d[2]^2)

    for i in 1:4
        ρ = gen_ρ(d[i])
        Pᵀρ =  Utils_states.take_Pᵀ(ρ, 2, d[i])
        @test Pᵀρ == ρ_test[i]
    end
    d = (3,2)

    ρ = reshape(1:36,6,6)
    @assert ρ == take_Pᵀ(ρ,0,d)
    ρᵀ¹ = take_Pᵀ(ρ,1,d)
    ρᵀ² = take_Pᵀ(ρ,2,d)
    @assert ρᵀ¹ == [1   2   3  19  20  21
                    7   8   9  25  26  27
                    13  14  15  31  32  33
                    4   5   6  22  23  24
                    10  11  12  28  29  30
                    16  17  18  34  35  36]
    @assert ρᵀ² == [  1   7  13   4  10  16
                      2   8  14   5  11  17
                      3   9  15   6  12  18
                      19  25  31  22  28  34
                      20  26  32  23  29  35
                      21  27  33  24  30  36]

end

@testset "isPPᵀ" begin
    ρ_1=   [ 0.0833333  0.0        0.0        0.0        0.0833333  0.0        0.0       0.0        0.0833333
             0.0        0.0833333  0.0        0.0        0.0        0.0        0.0       0.0        0.0
             0.0        0.0        0.0833333  0.0        0.0        0.0        0.0       0.0        0.0
             0.0        0.0        0.0        0.0833333  0.0        0.0        0.0       0.0        0.0
             0.0833333  0.0        0.0        0.0        0.0833333  0.0        0.0       0.0        0.0833333
             0.0        0.0        0.0        0.0        0.0        0.0833333  0.0       0.0        0.0
             0.0        0.0        0.0        0.0        0.0        0.0        0.208333  0.0        0.161374
             0.0        0.0        0.0        0.0        0.0        0.0        0.0       0.0833333  0.0
             0.0833333  0.0        0.0        0.0        0.0833333  0.0        0.161374  0.0        0.208333]

    ρ_2=   [ 0.5  0.0  0.0  0.0
             0.0  0.0  0.0  0.0
             0.0  0.0  0.0  0.0
             0.0  0.0  0.0  0.5]

    @test !Utils_states.isPPᵀ(ρ_1,(3,3))
    @test Utils_states.isPPᵀ(ρ_2,(2,2))

    ρ_true = ones(4,4)
    ρ_false = reshape(1:16,4,4)
    d = (2,2)
    @assert isPPᵀ(ρ_true,d) == true
    @assert isPPᵀ(ρ_false,d) == false

end

@testset "maketraceone" begin
    @test tr(Utils_states.maketraceone(reshape(1:81,9,9))) == 1
    @test tr(Utils_states.maketraceone(reshape(1:36,6,6))) == 1
    @test tr(Utils_states.maketraceone(rand(4,4))) ≈ 1
    @test tr(Utils_states.maketraceone(rand(4,4))) ≈ 1
    @test tr(Utils_states.maketraceone(rand(5,5))) ≈ 1
end

end

max_stars_repo_name: JAndriesJ/sep-rank-bounding

max_issues_repo_name: JAndriesJ/sep-rank-bounding

max_forks_repo_name: JAndriesJ/sep-rank-bounding


text:
module Density

# This implements different methods to compute density
#
# If you want to add a new method here, feel free to do so.
# Remember to also export the function name in GeoParams.jl (in addition to here)

using Parameters, Unitful, LaTeXStrings
using ..Units
using ..PhaseDiagrams
using GeoParams: AbstractMaterialParam, AbstractMaterialParamsStruct
using ..MaterialParameters: No_MaterialParam, MaterialParamsInfo
import Base.show, GeoParams.param_info

include("../Utils.jl")
include("../Computations.jl")

abstract type AbstractDensity{T} <: AbstractMaterialParam end

export  compute_density,        # calculation routines
        compute_density!,       # in place calculation
        param_info,             # info about the parameters
        AbstractDensity,
        ConstantDensity,        # constant
        PT_Density,             # P & T dependent density
        Compressible_Density    # Compressible density

# Define "empty" computational routines in case nothing is defined
compute_density!(rho::_T,s::No_MaterialParam{_T}; P::_T=zero(_T),T::_T=zero(_T)) where {_T} = zero(_T)
compute_density(s::No_MaterialParam{_T}; P::_T=zero(_T),T::_T=zero(_T)) where {_T} = zero(_T)

# Constant Density -------------------------------------------------------
"""
    ConstantDensity(ρ=2900kg/m^3)

Set a constant density:
```math
    \\rho  = cst
```
where ``\\rho`` is the density [``kg/m^3``].
"""
@with_kw_noshow struct ConstantDensity{_T,U}   <: AbstractDensity{_T}
    ρ::GeoUnit{_T,U}        =   2900.0kg/m^3                # density
end
ConstantDensity(args...) = ConstantDensity(convert.(GeoUnit,args)...)

function param_info(s::ConstantDensity) # info about the struct
    return MaterialParamsInfo(Equation = L"\rho = cst")
end

# Calculation routines
function compute_density(s::ConstantDensity{_T}, P::Quantity,T::Quantity) where _T
    @unpack_units ρ   = s
    return ρ
end

function compute_density(s::ConstantDensity{_T}, P::_T=zero(_T),T::_T=zero(_T)) where _T
    @unpack_val ρ   = s
    return ρ
end

function compute_density!(rho::AbstractArray{_T}, s::ConstantDensity{_T},P::_T=zero(_T),T::_T=zero(_T)) where _T
    @unpack_val ρ   = s
    rho[:] .= ρ
    return nothing
end

# Print info
function show(io::IO, g::ConstantDensity)
    print(io, "Constant density: ρ=$(UnitValue(g.ρ))")
end
#-------------------------------------------------------------------------

# Pressure & Temperature dependent density -------------------------------
"""
    PT_Density(ρ0=2900kg/m^3, α=3e-5/K, β=1e-9/Pa, T0=0C, P=0MPa)

Set a pressure and temperature-dependent density:
```math
    \\rho  = \\rho_0 (1.0 - \\alpha (T-T_0) + \\beta  (P-P_0) )
```
where ``\\rho_0`` is the density [``kg/m^3``] at reference temperature ``T_0`` and pressure ``P_0``,
``\\alpha`` is the temperature dependence of density and ``\\beta`` the pressure dependence.
"""
@with_kw_noshow struct PT_Density{_T,U1,U2,U3,U4,U5} <: AbstractDensity{_T}
    ρ0 ::GeoUnit{_T,U1}      =   2900.0kg/m^3                # density
    α  ::GeoUnit{_T,U2}      =   3e-5/K                      # T-dependence of density
    β  ::GeoUnit{_T,U3}      =   1e-9/Pa                     # P-dependence of density
    T0 ::GeoUnit{_T,U4}      =   0.0C                        # Reference temperature
    P0 ::GeoUnit{_T,U5}      =   0.0MPa                      # Reference pressure
end
PT_Density(args...) = PT_Density(convert.(GeoUnit,args)...)

function param_info(s::PT_Density)  # info
    return MaterialParamsInfo(Equation = L"\rho = \rho_0(1.0-\alpha (T-T_0) + \beta (P-P_0)" )
end

# Calculation routine in case units are provided
function compute_density(s::PT_Density{_T},P::Quantity,T::Quantity) where _T
    @unpack_units ρ0,α,β,P0, T0   = s

    ρ = ρ0*(1.0 - α*(T - T0) + β*(P - P0) )

    return ρ
end

# All other numbers
function compute_density(s::PT_Density{_T}, P::_T=zero(_T),T::_T=zero(_T)) where _T
    @unpack_val ρ0,α,β,P0, T0   = s

    ρ = ρ0*(1.0 - α*(T - T0) + β*(P - P0) )

    return ρ
end

function compute_density!(ρ::AbstractArray, s::PT_Density{_T}, P::_T=zero(_T),T::_T=zero(_T)) where _T
    @unpack ρ0,α,β,P0, T0   = s

    ρ .= ρ0*(1.0 - α*(T-T0) + β*(P-P0) )

    return nothing
end

# Print info
function show(io::IO, g::PT_Density)
    print(io, "P/T-dependent density: ρ0=$(UnitValue(g.ρ0)), α=$(UnitValue(g.α)), β=$(UnitValue(g.β)), T0=$(UnitValue(g.T0)), P0=$(UnitValue(g.P0))")
end
#-------------------------------------------------------------------------

# Pressure-dependent density -------------------------------
"""
    Compressible_Density(ρ0=2900kg/m^3, β=1e-9/Pa, P₀=0MPa)

Set a pressure-dependent density:
```math
    \\rho  = \\rho_0 \\exp(β*(P - P\\_0))
```
where ``\\rho_0`` is the density [``kg/m^3``] at reference pressure ``P_0`` and ``\\beta`` the pressure dependence.
"""
@with_kw_noshow struct Compressible_Density{_T,U1,U2,U3} <: AbstractDensity{_T}
    ρ0::GeoUnit{_T,U1}     =   2900.0kg/m^3                # density
    β ::GeoUnit{_T,U2}     =   1e-9/Pa                     # P-dependence of density
    P0::GeoUnit{_T,U3}     =   0.0MPa                      # Reference pressure
end
Compressible_Density(args...) = Compressible_Density(convert.(GeoUnit,args)...)

function param_info(s::Compressible_Density) # info about the struct
    return MaterialParamsInfo(Equation = L"\rho = \rho_0\exp(\beta*(P-P_0))"     )
end

function compute_density(s::Compressible_Density{_T}, P::_T=zero(_T),T::_T=zero(_T)) where _T
    @unpack_val ρ0,β,P0   = s

    ρ = ρ0*exp(β*(P - P0) )

    return ρ
end

function compute_density(s::Compressible_Density{_T}, P::Quantity, T::Quantity) where _T
    @unpack ρ0,β,P0   = s

    ρ = ρ0*exp(β*(P - P0) )

    return ρ
end

function compute_density!(ρ::_T, s::Compressible_Density{_T}, P::_T=zero(_T),T::_T=zero(_T)) where _T
    @unpack ρ0,β,P0   = s

    return ρ0*exp( β*(P-P0) )
end

function compute_density!(ρ::AbstractArray, s::Compressible_Density{_T}, P::_T=zero(_T),T::_T=zero(_T)) where _T
    @unpack ρ0,β,P0   = s

    ρ .= ρ0*exp( β*(P-P0) )

    return nothing
end

# Print info
function show(io::IO, g::Compressible_Density)
    print(io, "Compressible density: ρ0=$(UnitValue(g.ρ0)), β=$(UnitValue(g.β)), P0=$(UnitValue(g.P0))")
end
#-------------------------------------------------------------------------

#-------------------------------------------------------------------------
# Phase diagrams
function param_info(s::PhaseDiagram_LookupTable) # info about the struct
    return MaterialParamsInfo(Equation = L"\rho = f_{PhaseDiagram}(T,P))" )
end

"""
    compute_density(P,T, s::PhaseDiagram_LookupTable)
Interpolates density as a function of `T,P` from a lookup table
"""
function compute_density(s::PhaseDiagram_LookupTable, P::_T=zero(_T),T::_T=zero(_T)) where _T
    return s.Rho(T,P)
end

"""
    compute_density!(rho::AbstractArray{<:AbstractFloat}, P::AbstractArray{<:AbstractFloat},T::AbstractArray{<:AbstractFloat}, s::PhaseDiagram_LookupTable)
In-place computation of density as a function of `T,P`, in case we are using a lookup table.
"""
function compute_density!(rho::AbstractArray{_T}, s::PhaseDiagram_LookupTable, P::AbstractArray{_T}=[zero(_T)],T::AbstractArray{_T}=[zero(_T)]) where _T
    rho[:] = s.Rho.(T,P)
    return nothing
end

#------------------------------------------------------------------------------------------------------------------#
# Computational routines needed for computations with the MaterialParams structure

# This assumes that density always has a single parameter. If that is not the case, we will have to extend this (to be done)
function compute_density(s::AbstractMaterialParamsStruct, P::_T=zero(_T),T::_T=zero(_T)) where {_T}
    return compute_density(s.Density[1], P,T)
end

# these routines may come handy when we have >1 field for Density
#function compute_density(s::NTuple{N,AbstractDensity{_T}}, P::_T=zero(_T), T::_T=zero(_T)) where {N,_T}
#    compute_density.(s,P,T)
#end

#now with Tuple of Tuples
#function compute_density!(rho::Vector{NTuple{M,_T}}, P::Number, T::Number, MatParam::NTuple{N, NTuple{M, AbstractMaterialParamsStruct}}) where {N,M,_T}
#    rho .= map(x->compute_density(P,T,x), MatParam)
#end

#-------------------------------------------------------------------------------------------------------------

"""
    compute_density!(rho::AbstractArray{_T, ndim}, MatParam::NTuple{N,AbstractMaterialParamsStruct}, Phases::AbstractArray{_I, ndim}; P=nothing, T=nothing) where {ndim,N,_T,_I<:Integer}

In-place computation of density `rho` for the whole domain and all phases, in case a vector with phase properties `MatParam` is provided, along with `P` and `T` arrays.
This assumes that the `Phase` of every point is specified as an Integer in the `Phases` array.
# Example
```julia
julia> MatParam = (SetMaterialParams(Name="Mantle", Phase=1,
                        CreepLaws= (PowerlawViscous(), LinearViscous(η=1e23Pa*s)),
                        Density   = PT_Density()
                        ),
                    SetMaterialParams(Name="Crust", Phase=2,
                        CreepLaws= (PowerlawViscous(), LinearViscous(η=1e23Pas)),
                        Density   = ConstantDensity(ρ=2900kg/m^3))
                  );
julia> Phases = ones(Int64,400,400);
julia> Phases[:,20:end] .= 2
julia> rho     = zeros(size(Phases))
julia> T       =  ones(size(Phases))
julia> P       =  ones(size(Phases))*10
julia> compute_density!(rho, MatParam, Phases, P, T)
julia> rho
400×400 Matrix{Float64}:
2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  …  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0
 2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91     2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0
 2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91     2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0
 2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91     2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0
 2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91     2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0
 2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  …  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0
 2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91     2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0
 2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91     2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0
 2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91     2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0
 2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91     2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0
 2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  …  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0
 2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91     2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0
 2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91     2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0
 2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91     2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0
    ⋮                                            ⋮                                         ⋱     ⋮                                       ⋮
 2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91     2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0
 2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91     2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0
 2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91     2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0
 2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  …  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0
 2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91     2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0
 2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91     2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0
 2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91     2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0
 2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91     2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0
 2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  …  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0
 2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91     2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0
 2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91     2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0
 2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91     2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0
 2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91  2899.91     2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0  2900.0
```
The routine is made to minimize allocations:
```julia
julia> using BenchmarkTools
julia> @btime compute_density!(\$rho, \$MatParam, \$Phases, P=\$P, T=\$T)
    203.468 μs (0 allocations: 0 bytes)
```
_____________________________________________________________________________________________________________________________

    compute_density!(rho::AbstractArray{_T, N}, MatParam::NTuple{K,AbstractMaterialParamsStruct}, PhaseRatios::AbstractArray{_T, M}, P=nothing, T=nothing)

In-place computation of density `rho` for the whole domain and all phases, in case a vector with phase properties `MatParam` is provided, along with `P` and `T` arrays.
This assumes that the `PhaseRatio` of every point is specified as an Integer in the `PhaseRatios` array, which has one dimension more than the data arrays (and has a phase fraction between 0-1)
"""
compute_density!(args...) = compute_param!(compute_density,args...) #Multiple dispatch to rest of routines found in Computations.jl
compute_density(args...) = compute_param(compute_density,args...)

#=
function compute_density!(rho::AbstractArray{_T, ndim}, MatParam::NTuple{N,AbstractMaterialParamsStruct}, Phases::AbstractArray{_I, ndim}, P=nothing, T=nothing) where {ndim,N,_T,_I<:Integer}

    Phase_tup = ntuple(i->MatParam[i].Phase, Val(N))

    # check if we compute with P/T or use default values
    isnothing(T) ? compute_T = false : compute_T = true
    isnothing(P) ? compute_P = false : compute_P = true

    Tval = zero(_T)
    Pval = zero(_T)

    @inbounds for I in eachindex(Phases)
        phase   = find_ind(Phase_tup, Phases[I])

        # Extract relevant value if requested
        if compute_T; Tval = T[I]; end
        if compute_P; Pval = P[I]; end

        # this computes density for ALL phases, which is a bit of an overkill as we only need density for a single phase
        rho_tup = compute_density(MatParam, Pval, Tval)
        rho[I]  = rho_tup[phase]
    end
end

function compute_density!(rho::AbstractArray{_T, N}, MatParam::NTuple{K,AbstractMaterialParamsStruct}, PhaseRatios::AbstractArray{_T, M}, P=nothing, T=nothing) where {_T<:AbstractFloat, N,M, K}
    if M!=(N+1)
        error("The PhaseRatios array should have one dimension more than the other arrays")
    end

    # check if we compute with P/T or use default values
    isnothing(T) ? compute_T = false : compute_T = true
    isnothing(P) ? compute_P = false : compute_P = true

    Tval = zero(_T)
    Pval = zero(_T)
    if compute_P
        Rindex = CartesianIndices(P)
    else
        Rindex = CartesianIndices(T)
    end

    @inbounds for I in Rindex
        frac    = view(PhaseRatios, Tuple(I)..., 1:K)    # fraction of each phase @ point I

        # Extract relevant value if requested
        if compute_T; Tval = T[I]; end
        if compute_P; Pval = P[I]; end

        # compute point-wise density:
        rho[I] = compute_density_times_frac(frac,MatParam, Pval, Tval)

    end

    return
end

# Multiplies density with the fraction of a phase
function compute_density_times_frac(PhaseRatios::AbstractArray{_T, 1}, MatParam::NTuple{N,AbstractMaterialParamsStruct}, P::_T, T::_T) where {_T, N}

    value_tup = compute_density(MatParam, P, T)

    # Sum & multiply density with fraction
    val = zero(_T)
    @inbounds for j=1:N
        val += PhaseRatios[j]*value_tup[j]
    end

    return val
end
=#

end

# OBSOLETE routines; I leave them in for now (commented), in case we want to reuse pieces of that @ a later stage.
# much of what we have above is more efficient (apart when using the Phase Diagram lookup tables)
#=
# OLD function (which actually works better in case of phase diagrams as it can do computations at omce)
function compute_density!(rho::AbstractArray{<:AbstractFloat, N}, PhaseRatios::AbstractArray{<:AbstractFloat, M}, P::AbstractArray{<:AbstractFloat, N},T::AbstractArray{<:AbstractFloat, N}, MatParam::AbstractArray{<:AbstractMaterialParamsStruct, 1}) where {N,M}

    if M!=(N+1)
        error("The PhaseRatios array should have one dimension more than the other arrays")
    end

    rho1        =  zeros(size(rho))
    rho         .=  0.0;
    for i = 1:length(MatParam)
        Fraction    = selectdim(PhaseRatios,M,i);
        if (!isempty(MatParam[i].Density))
            ind         =   Fraction .> 0.0
            rho_local   =   view(rho1, ind )
            P_local     =   view(P   , ind )
            T_local     =   view(T   , ind )
            density = MatParam[i].Density[1]
            compute_density!(rho_local, P_local, T_local, density )
            rho[ind] .+= rho_local.*Fraction[ind]
        end

    end
end
# OBSOLETE?
function compute_density!(rho::AbstractArray{<:AbstractFloat, N}, Phases::AbstractArray{<:Integer, N}, P::AbstractArray{<:AbstractFloat, N},T::AbstractArray{<:AbstractFloat, N}, MatParam::AbstractArray{<:AbstractMaterialParamsStruct, 1}) where {N,_T}
    for i = 1:length(MatParam)
        if !isempty(MatParam[i].Density)
            # Create views into arrays (so we don't have to allocate)
            ind = Phases .== MatParam[i].Phase
            rho_local   =   view(rho, ind )
            P_local     =   view(P  , ind )
            T_local     =   view(T  , ind )
           # density::Union{AbstractDensity{_T},PhaseDiagram_LookupTable}     =   MatParam[i].Density[1]
            density    =   MatParam[i].Density[1]

            #@time compute_density!(rho_local, P_local, T_local, dens )
            compute_density!(rho_local, P_local, T_local, density )

        end

    end
end
# Many allocations
function compute_density!(rho::AbstractArray{_T, ndim}, Phases::AbstractArray{<:Integer, ndim}, P::AbstractArray{_T, ndim},T::AbstractArray{_T, ndim}, MatParam::NTuple{N,AbstractMaterialParamsStruct}) where {ndim,N,_T}
    # This is the general way to implement this.
    #  Remarks:
    #       1) It allocates a lot when used with a phase diagram
    #       2) We still have to check if this works with the way ParallelStencil operates

    # The phases in MatParam may be ordered differently (or start with zero)

    Phase_vec = [MatParam[i].Phase for i=1:N]
    Phase_ind = copy(Phases);
    for i=1:N
        Phase_ind[Phases.==Phase_vec[i]] .= i
    end

    rho_local = zeros(_T,N)
    for i in eachindex(Phase_ind)
        phase   =   Phase_ind[i]

        # This does not allocate but computes density for all phases simultaneously.
        #  That is a bit of an overkill, but alternative approaches appear to allocate
        compute_density!(rho_local,P[i],T[i], MatParam)
        rho[i]  =   rho_local[phase]
    end
    return
end
=#


text:
using Test

using CSV
using DataFrames
using FlightMechanicsUtils
using StaticArrays

using FlightMechanicsSimulator

df = DataFrame(CSV.File("data/sixdof.csv"))
# Test file was wrong before and longitudinal load factor
# needs to be corrected
df[!, "o3"] = df[!, "o3"] / (FlightMechanicsSimulator.F16Stevens.MASS * KG2LB / F16Stevens.GD)

for case in eachrow(df)
    x = Array(case[["x$ii" for ii in 1:13]])

    x[1] = x[1] * FT2M
    x[12] = -x[12] * FT2M

    controls = Array(case[["c$ii" for ii in 1:4]])

    dssd, outputs1 =
        f(
            case.time,
            SixDOFAeroEuler(x),
            controls,
            F16(F16Stevens.MASS, F16Stevens.INERTIA, case.xcg),
            F16StevensAtmosphere(-x[12]),
            LHDownGravity(FlightMechanicsSimulator.F16Stevens.GD*FT2M),
        )

    xd1 = get_xdot(dssd)  # This is a SVector
    xd1 = Array(xd1)  # Allow modification of xd1
    xd1[1] *= M2FT
    xd1[10] *= M2FT
    xd1[11] *= M2FT
    xd1[12] *= -M2FT

    outputs1[4] *= PA2PSF

    @test isapprox(
        xd1,
        Array(case[["x$(ii)_dot" for ii in 1:13]]),
        nans = true,
        atol = 1e-10,
    )

    @test isapprox(
        outputs1,
        Array(case[["o$(ii)" for ii in 1:7]]),
        nans = true,
        atol = 1e-10,
    )
end


text:
"""
    optimize!(fock[, ::Type{Optimizer}=BFGS; kwargs...])

Solve the Hartree–Fock problem via non-linear optimization. The
algorithm, specified by `Optimizer`, has to be a first-order algorithm
that supports optimization on a manifold, the default being
`Optim.BFGS`. The computation of the gradient is accomplished by
applying the `fock` operator onto test vectors (similar to how
[`scf!`](@ref) performs Krylov iterations).

"""
function optimize!(fun!::Function, fock::Fock, ::Type{Optimizer}=BFGS;
                   opt_iters=1000, f_tol=0.0, g_tol=1e-8,
                   scf_iters=200, scf_tol=1e-3, scf_method=:lobpcg,
                   update_mixing_coefficients=true,
                   linesearch=LineSearches.HagerZhang(),
                   verbosity=2, num_printouts=typemax(Int),
                   kwargs...) where {Optimizer<:Optim.FirstOrderOptimizer}
    P = orbitals(fock.quantum_system)
    c = coefficients(fock.quantum_system)

    nc = length(c)

    H = spzeros(eltype(c), nc, nc)
    # Kinetic energy matrix
    T = spzeros(eltype(c), nc, nc)

    kws = [KrylovWrapper(hamiltonian(eq))
           for eq in fock.equations]
    f = FockProblem(fock, P, c, H, kws)
    manif = setup_manifold(f)

    trace,tolerance,_,eng,virial,flags = setup_solver_trace(
        verbosity, opt_iters, g_tol, 0, num_printouts,
        tol_header="|g|")

    trace_callback = opt_state -> begin
        # TODO: Think about moving secular problem to optimization.
        solve_secular_problem!(H, c, fock, update_mixing_coefficients)

        fun!(P, c)

        if !isnothing(trace)
            tolerance.current = opt_state.g_norm

            Etot = opt_state.value
            energy_matrix!(T, fock, :kinetic_energy)
            ET = c'T*c
            EV = Etot-ET
            eng[1].E = real(Etot)
            eng[2].E = real(ET)
            eng[3].E = real(EV)
            virial.V = real(EV/ET)

            SolverTraces.next!(trace)
        end

        false
    end

    options = Optim.Options(iterations=opt_iters,
                            f_tol=f_tol,
                            g_tol=g_tol,
                            allow_f_increases=true,
                            callback=trace_callback)

    optimizer = Optimizer(manifold=manif, linesearch=linesearch)

    t₀ = time()

    if scf_iters > 0
        @info "Performing initial SCF iterations"
        scf!(fun!, fock;
             max_iter=scf_iters, tol=scf_tol,
             method=scf_method,
             update_mixing_coefficients=update_mixing_coefficients,
             verbosity=verbosity,
             num_printouts=typemax(Int),
             kwargs...)
    end

    isnothing(trace) || print_header(trace)
    o = optimize(f, (w,v) -> jac!(w,v,f), copy(P),
                 optimizer, options)
    verbosity > 1 && display(o)
    copyto!(P, o.minimizer)

    rayleigh_ritz!(P, fock, kws; verbosity=verbosity)

    elapsed = time() - t₀
    verbosity > 0 && println("Finished in $(elapsed) seconds")

    analyze_symmetry_orbitals(fock, P, kws, verbosity=verbosity)

    fock
end

optimize!(fock::Fock, args...; kwargs...) =
    optimize!((_,_)->nothing, fock, args...; kwargs...)

export optimize!


text:
struct MatrixSequence{T} # <:  Vector{AbstractMatrix{T}}
    sequence:: Array{Array{T,2},1}
    interspersion:: Array{Array{T,2},1}
    size::Int
    function MatrixSequence{T}(sequence::Array{Array{T,2},1}, interspersion::Array{Array{T,2},1}) where T
        if (length(sequence) < 1)
            return error("not a sequence")
        end
        if (length(interspersion) != length(sequence) + 1 )
            return error("not an interspersion")
        end
        if size(sequence[1])[1] != size(interspersion[1])[1]
            return error("inconsitent sizes")
        end
        for matrix in sequence
            if !(LinearAlgebraUtilities.isSquare(matrix))
                return error("not square")
            end
            if size(matrix) != size(sequence[1])
                return error("inconsitent sizes")
            end
        end
        for matrix in interspersion
            if !(LinearAlgebraUtilities.isSquare(matrix))
                return error("not square")
            end
            if size(matrix) != size(sequence[1])
                return error("inconsitent sizes")
            end
        end

        return new{T}(
            map(AbstractMatrix{T}, sequence),
            map(AbstractMatrix{T}, interspersion),
            size(sequence[1])[1]
        )
    end
end
function MatrixSequence(sequence::Array{Array{T,2},1}, interspersion) where T
    return MatrixSequence{T}(sequence, Array{Array{T,2}, 1}(interspersion))
end
function MatrixSequence(sequence::Array{Array{T,2}, 1}) where T
    return MatrixSequence{T}(sequence, Array{Array{T,2}, 1}([LinearAlgebra.I(size(sequence[1])[1]) for i in 1:size(sequence)[1]+1]))
end
function MatrixSequence(matrixSequence::MatrixSequence)
    matrixSequence
end
function Base.convert(::Type{MatrixSequence}, matrixSequence::MatrixSequence)
    matrixSequence
end
function Base.convert(::Type{AbstractMatrix}, matrixSequence::MatrixSequence)
    Matrix(matrixSequence)
end
function Base.size(matrixSequence::MatrixSequence, dim::Integer)
    size(matrixSequence.sequence, dim)
end
function Base.size(matrixSequence::MatrixSequence)
    size(matrixSequence.sequence)
end
function Base.length(matrixSequence::MatrixSequence)
    return length(matrixSequence.sequence)
end
function Base.Matrix(matrixSequence::MatrixSequence)::AbstractMatrix
    sequence = matrixSequence.sequence
    interspersion =  matrixSequence.interspersion
    return foldl(
        (cur, i)-> cur * sequence[i]*interspersion[i+1],
        1:size(matrixSequence)[1];
        init = interspersion[1])
end
function sliceInner(matrixSequence::MatrixSequence, i::Integer, j::Integer)::MatrixSequence
    sequence, interspersion, Size = matrixSequence.sequence, matrixSequence.interspersion, matrixSequence.size
    return MatrixSequence(sequence[i:j], [LinearAlgebra.I(Size),  interspersion[i+1:j]..., LinearAlgebra.I(Size)])
end
function slice(matrixSequence::MatrixSequence, i::Integer, j::Integer)::MatrixSequence
    sequence, interspersion, Size = matrixSequence.sequence, matrixSequence.interspersion, matrixSequence.size
    return MatrixSequence(sequence[i:j], interspersion[i:j+1])
end
function Base.:*(MS1::MatrixSequence, MS2::MatrixSequence)
    sequence1, inspersion1 = MS1.sequence, MS1.interspersion
    sequence2, inspersion2 = MS2.sequence, MS2.interspersion
    sequence = [sequence1 ; sequence2]
    inspersion = [inspersion1[1:end - 1] ; [ inspersion1[end] * inspersion2[1] ] ; inspersion2[2:end]]
    return MatrixSequence(sequence, inspersion)
end
function setInterspersion(matrix::AbstractMatrix, matrixSequence::MatrixSequence, i::Integer)::MatrixSequence
    sequence = matrixSequence.sequence
    interspersion = matrixSequence.interspersion
    matrixSequence.interspersion[i] = matrix
    return MatrixSequence(sequence, interspersion)
end
function setSequence(matrix::AbstractMatrix, matrixSequence::MatrixSequence, i::Integer)::MatrixSequence
    sequence = matrixSequence.sequence
    interspersion = matrixSequence.interspersion
    matrixSequence.sequence[i] = matrix
    return MatrixSequence(sequence, interspersion)
end
function replaceSequence(subMatrixSequence::MatrixSequence, matrixSequence::MatrixSequence, i::Integer)
    subSequence, subInterspersion = subMatrixSequence.sequence, subMatrixSequence.interspersion
    sequence, interspersion = matrixSequence.sequence, matrixSequence.interspersion
    return MatrixSequence(
        [sequence[1:i-1] ; subSequence ; sequence[i+1:end]],
        [interspersion[1:i-1] ; [interspersion[i]*subInterspersion[1]] ; subInterspersion[2:end-1] ; [subInterspersion[end]*interspersion[i+1]] ; interspersion[i+2:end]]
    )
end


text:
using JuMP, EAGO

                     m = Model()

                     EAGO.register_eago_operators!(m)

                     @variable(m, -1 <= x[i=1:4] <= 1)

                     @variable(m, -23.072345451695163 <= q <= 18.38163265505858)

                     add_NL_constraint(m, :(swish(-0.6895881478464707 + 0.42824131807966515*swish(-0.9657925733535908 + -0.20993433529810623*$(x[1]) + -0.9858887282433288*$(x[2]) + -0.7926811137744871*$(x[3]) + -0.8802726339564089*$(x[4])) + -0.8758217593243995*swish(-0.29556778373435044 + 0.6695148008497993*$(x[1]) + 0.7620585252555783*$(x[2]) + 0.8362720647957991*$(x[3]) + 0.8751574333999192*$(x[4])) + -0.3819126296826152*swish(-0.8627823044260601 + -0.3188988197195952*$(x[1]) + -0.18058802482760727*$(x[2]) + -0.7255181409249616*$(x[3]) + -0.8658172698743405*$(x[4])) + -0.7523924735752416*swish(0.5506348394687617 + 0.9730675890304568*$(x[1]) + 0.0064031244486910666*$(x[2]) + -0.590531971380631*$(x[3]) + -0.964793293764588*$(x[4]))) + swish(0.5903696565764314 + 0.6045010784464115*swish(-0.9657925733535908 + -0.20993433529810623*$(x[1]) + -0.9858887282433288*$(x[2]) + -0.7926811137744871*$(x[3]) + -0.8802726339564089*$(x[4])) + 0.19992590084675577*swish(-0.29556778373435044 + 0.6695148008497993*$(x[1]) + 0.7620585252555783*$(x[2]) + 0.8362720647957991*$(x[3]) + 0.8751574333999192*$(x[4])) + -0.13493555263327828*swish(-0.8627823044260601 + -0.3188988197195952*$(x[1]) + -0.18058802482760727*$(x[2]) + -0.7255181409249616*$(x[3]) + -0.8658172698743405*$(x[4])) + 0.0007730145672497635*swish(0.5506348394687617 + 0.9730675890304568*$(x[1]) + 0.0064031244486910666*$(x[2]) + -0.590531971380631*$(x[3]) + -0.964793293764588*$(x[4]))) + swish(-0.987583954230804 + 0.6297203029880079*swish(-0.9657925733535908 + -0.20993433529810623*$(x[1]) + -0.9858887282433288*$(x[2]) + -0.7926811137744871*$(x[3]) + -0.8802726339564089*$(x[4])) + -0.939181196020844*swish(-0.29556778373435044 + 0.6695148008497993*$(x[1]) + 0.7620585252555783*$(x[2]) + 0.8362720647957991*$(x[3]) + 0.8751574333999192*$(x[4])) + 0.23747324715066886*swish(-0.8627823044260601 + -0.3188988197195952*$(x[1]) + -0.18058802482760727*$(x[2]) + -0.7255181409249616*$(x[3]) + -0.8658172698743405*$(x[4])) + 0.2556344288953589*swish(0.5506348394687617 + 0.9730675890304568*$(x[1]) + 0.0064031244486910666*$(x[2]) + -0.590531971380631*$(x[3]) + -0.964793293764588*$(x[4]))) + swish(-0.44105426128556546 + -0.39452594471110514*swish(-0.9657925733535908 + -0.20993433529810623*$(x[1]) + -0.9858887282433288*$(x[2]) + -0.7926811137744871*$(x[3]) + -0.8802726339564089*$(x[4])) + 0.16620534874676274*swish(-0.29556778373435044 + 0.6695148008497993*$(x[1]) + 0.7620585252555783*$(x[2]) + 0.8362720647957991*$(x[3]) + 0.8751574333999192*$(x[4])) + 0.6245292013722641*swish(-0.8627823044260601 + -0.3188988197195952*$(x[1]) + -0.18058802482760727*$(x[2]) + -0.7255181409249616*$(x[3]) + -0.8658172698743405*$(x[4])) + 0.9983459126930239*swish(0.5506348394687617 + 0.9730675890304568*$(x[1]) + 0.0064031244486910666*$(x[2]) + -0.590531971380631*$(x[3]) + -0.964793293764588*$(x[4]))) - $q <= 0.0))

                     @objective(m, Min, q)

                     return m

max_stars_repo_name: PSORLab/RSActivationFunctions

max_issues_repo_name: PSORLab/RSActivationFunctions

max_forks_repo_name: PSORLab/RSActivationFunctions


text:
"""
    AbstractOptimizer

Abstract type of solvers
"""
abstract type AbstractOptimizer end

"""
    run!

Abstract function of running algorithm
"""
function run! end

include("algorithms/common.jl")
include("algorithms/merit.jl")
include("algorithms/subproblem.jl")
include("algorithms/slp.jl")

max_issues_repo_name: exanauts/ActiveSetMethods


text:
import ModelingToolkit.SymbolicUtils.FnType

function _generate_deqs(x::Basis, states, iv, p)
    @assert length(x) == length(states)
    # Create new variables with time dependency
    ∂t = Differential(iv)
    dvs = [Num(Sym{FnType{Tuple{Any}, Real}}(_get_name(xi))(value(iv))) for xi in states]
    dvsdt = ∂t.(dvs)
    # Adapt equations
    eqs = dvsdt .~ x(dvs,p,iv)
    return eqs, dvs
end

"""
$(SIGNATURES)

Convert a given `Basis` or `SparseIdentificationResult` into an `ODESystem`. For details, see ModelingToolkit.jl.
"""
function ModelingToolkit.ODESystem(x::Basis, iv = nothing, dvs = Num[], ps = Num[]; observed = Num[], systems = ODESystem[],kwargs...)
    iv = isnothing(iv) ? independent_variable(x) : iv
    dvs = isempty(dvs) ? variables(x) : dvs
    ps = isempty(ps) ? parameters(x) : ps
    eqs, dvs = _generate_deqs(x, dvs, iv, ps)
    observed = isempty(observed) ? x.observed : observed
    systems = isempty(systems) ? x.systems : systems
    return ODESystem(
        eqs, iv, dvs, ps,
        observed = observed, systems = systems, kwargs...)
end

function ModelingToolkit.ODESystem(b::SparseIdentificationResult, iv = nothing, dvs = Num[], ps = Num[]; kwargs...)
    return ODESystem(b.equations, iv, dvs, ps; kwargs...)
end

function _remove_controls(states, controls)
    idxs = ones(Bool, size(states)...)
    for i in eachindex(states)
        idxs[i] = !any([isequal(states[i], ci) for ci in controls])
    end
    return states[idxs]
end

function _generate_deqs(x::Basis, states, iv, p, controls)
    # Create new variables with time dependency
    states_ = _remove_controls(states, controls)
    @assert length(x) == length(states_)
    ∂t = Differential(iv)
    dvs = [Num(Sym{FnType{Tuple{Any}, Real}}(_get_name(xi))(value(iv))) for xi in states_]
    input_states = _create_input_vec(states, dvs, controls)
    dvsdt = ∂t.(dvs)
    # Adapt equations
    eqs = dvsdt .~ x(input_states,p, iv)
    return eqs, dvs, input_states
end

function _create_input_vec(states, dvs, controls)
    input_states = Array{Any}(undef, size(states)...)
    state_idx = ones(Bool, size(states)...)
    control_idx = zeros(Bool, size(states))
    for i in eachindex(states)
        for k in eachindex(controls)
            if isequal(states[i], controls[k])
                control_idx[i] = true
                state_idx[i] = false
            end
        end
    end
    input_states[state_idx] .= dvs
    input_states[control_idx] .= controls
    return input_states
end

"""
$(SIGNATURES)

Convert a given `Basis` or `SparseIdentificationResult` into a `ControlSystem`. For details, see ModelingToolkit.jl.
"""
function ModelingToolkit.ControlSystem(loss, x::Basis, controls, iv = nothing, dvs = nothing, ps = nothing;
    observed = Num[], systems = ODESystem[], kwargs...)
    iv = isnothing(iv) ? independent_variable(x) : iv
    dvs = isnothing(dvs) ? variables(x) : dvs
    ps = isnothing(ps) ? parameters(x) : ps
    eqs, dvs, input_states = _generate_deqs(x, dvs, iv, ps, controls)
    #return input_states
    subs = [(xi => is) for (xi, is) in zip(variables(x), input_states)]
    loss = substitute.(loss, (subs,))[1]
    observed = isempty(observed) ? x.observed : observed
    systems = isempty(systems) ? x.systems : systems
    return ControlSystem(loss, eqs, iv, dvs, controls, ps,
                         observed = observed, systems = systems, kwargs...)
end

function ModelingToolkit.ControlSystem(loss, b::SparseIdentificationResult, controls, iv = nothing, dvs = Num[], ps = Num[]; kwargs...)
    return ControlSystem(loss, b.equations, controls, iv, dvs, ps; kwargs...)
end


text:
"""
    TPFlashMethod

Abstract type for `tp_flash` routines. it requires defining `numphases(method)` and `tp_flash_impl(model,p,T,n,method)`.

"""
abstract type TPFlashMethod end

"""
tp_flash(model, p, T, n, method::TPFlashMethod =DETPFlash())

Routine to solve non-reactive multicomponent flash problem.
The default method uses Global Optimization. see [DETPFlash](@ref)

Inputs:
 - T, Temperature
 - p, Pressure
 - n, vector of number of moles of each species

Outputs - Tuple containing:
 - xᵢⱼ, Array of mole fractions of species j in phase i
 - nᵢⱼ, Array of mole numbers of species j in phase i, [mol]
 - G, Gibbs Free Energy of Equilibrium Mixture [J]
"""
function tp_flash end

"""
    numphases(method::TPFlashMethod)

return the number of phases supported by the TP flash method. by default its set to 2.
it the method allows it, you can set the number of phases by doing `method(;numphases = n)`.
"""
numphases(method::TPFlashMethod) = 2

#default
include("tp_flash/DifferentialEvolutiontp_flash.jl")
include("tp_flash/RachfordRicetp_flash.jl")

function tp_flash(model::EoSModel, p, T, n,method::TPFlashMethod = DETPFlash())
    numspecies = length(model)
    if numspecies != length(n)
        error("There are ", numspecies,
            " species in the model, but the number of mole numbers specified is ",
            length(n))
    end
    if numphases(method) == 1
        return (n, n / sum(n), gibbs_free_energy(model, p, T, n))
    end
    return tp_flash_impl(model::EoSModel,p,T,n,method)
end

export tp_flash


text:
module UndefinedPositive

using PublicAPI: @public
@public WILL_BE_DEFINED
@public WILL_NOT_BE_DEFINED

const WILL_BE_DEFINED = 1

end


text:
"""
    abstract type WebsocketError <: Exception
WebsocketError child error types have the following fields:
- msg::String
- log::Function

The `log()` function will internally call: @error msg  exception = (err, trace)

Where `trace` is the backtrace of the exception origin.
"""
abstract type WebsocketError <: Exception end
function msg(err::Exception)
    try
        return err.msg
    catch
        return string(typeof(err))
    end
    # hasfield(typeof(err), :msg) ? err.msg : string(typeof(err)) #hasfield needs julia >= 1.2
end
logError(self::Exception, err::Exception, trace::Array) = @error string(typeof(self))*"(\"$(self.msg)\")" exception = (err, trace)

"""
    struct ConnectError <: WebsocketError
An exception originated while trying to start a server or connect a client to a server
"""
struct ConnectError <: WebsocketError
    msg::String
    log::Function
    function ConnectError(err::Exception, trace::Array{Union{Ptr{Nothing}, Base.InterpreterIP},1} = [])
        self = new(
            msg(err),
            () -> logError(self, err, trace)
        )
    end
end
"""
    struct PeerConnectError <: WebsocketError
The server made an exception when a client tried to connect
"""
struct PeerConnectError <: WebsocketError
    msg::String
    log::Function
    function PeerConnectError(err::Exception, trace::Array{Union{Ptr{Nothing}, Base.InterpreterIP},1} = [])
        self = new(
            msg(err),
            () -> logError(self, err, trace)
        )
    end
end
"""
    struct CallbackError <: WebsocketError
An exception originated in a user provided callback function
"""
struct CallbackError <: WebsocketError
    msg::String
    log::Function
    function CallbackError(err::Exception, trace::Array{Union{Ptr{Nothing}, Base.InterpreterIP},1} = [])
        self = new(
            msg(err),
            () -> logError(self, err, trace)
        )
    end
end
"""
    struct FrameError <: WebsocketError
An exception originated while parsing a websocket data frame
"""
struct FrameError <: WebsocketError
    msg::String
    log::Function
    function FrameError(err::Exception, trace::Array{Union{Ptr{Nothing}, Base.InterpreterIP},1} = [])
        self = new(
            msg(err),
            () -> logError(self, err, trace)
        )
    end
end

function Base.showerror(io::IO, e::WebsocketError)
    print(io, "$(typeof(e)): ")
    print(io, e.msg)
end

max_stars_repo_name: citkane/SimpleWebsockets

max_issues_repo_name: citkane/SimpleWebsockets

max_forks_repo_name: citkane/SimpleWebsockets


text:
for N in 2:9
    facts("Cross polytope in $N dimensions") do
        m = Model()

        @variable(m, x[1:N])

        for k in 0:N
            for S in combinations(1:N, k)
                Sᶜ = setdiff(1:N, S)
                @constraint(m, sum(x[i] for i in S) - sum(x[i] for i in Sᶜ) ≤ 1)
            end
        end

        v, r = get_extrema(m)

        @fact length(v) --> 2N
        @fact length(r) --> 0

        for i in 1:N
            ex = zeros(N)
            ex[i] = 1
            @fact is_approx_included(v, ex) --> true
            ex[i] = -1
            @fact is_approx_included(v, ex) --> true
        end
    end
end


text:
### Instruction:
# Run before main.jl.
# CSV files must be in the input folder matching the simulatin model.
# For the list of input parameters, see readData function in Helper.jl
# For the list of input files, check all *.csv files in Helper.jl
using Distributed
@everywhere include("incl.jl")
@everywhere sayhello()
@everywhere push!(LOAD_PATH,pwd())

@everywhere begin
using TypeDef, Helper, Tools, Compute, Solve, FOCs, UserFileIO, MonteCarlo
using Post
using JLD, Printf
end

model =   0
mcid  =  20
b1    =   1
b2    = 200

### Monte Carlo analysis
for vInt=[false,true]
  @printf("\n=================== Starting Monte Carlo ===================\n")
  @printf("  Model: %2d  MCid: %2d  Vertical Int: %d\n",model,mcid,vInt)
  dat,dim,env,mat,par0,set,sim,indx,mask = readData(model,mcid,b1,b2,vInt,true)
  eqa = simMonteCarloMulti!(b1,b2,dat,dim,env,mat,par0,set,indx,mask,vInt)
end

max_stars_repo_name: yutec/FightBrand

max_issues_repo_name: yutec/FightBrand

max_forks_repo_name: yutec/FightBrand


text:
function simulate(
		model::CartpoleFriction,
		policy,
		K,
		z_nom, u_nom,
		Q, R,
		T_sim, Δt,
		z0, w;
        _norm = 2,
        ul = -Inf * ones(length(u_nom[1])),
        uu = Inf * ones(length(u_nom[1])),
        friction = false,
        μ = 0.1)

    T = length(K) + 1
    times = [(t-1) * Δt for t = 1:T-1]
    tf = Δt * T
    t_sim = range(0, stop = tf, length = T_sim)
    t_ctrl = range(0, stop = tf, length = T)
    dt_sim = tf / (T_sim - 1)

    p = 1:policy.output

    A_state = hcat(z_nom...)
    A_ctrl = hcat(u_nom...)

    z_rollout = [z0]
    u_rollout = []
    J = 0.0
    Jx = 0.0
    Ju = 0.0
    for tt = 1:T_sim-1
        t = t_sim[tt]
        k = searchsortedlast(times, t)

        z_cubic = zeros(model.n)
        for i = 1:model.n
            interp_cubic = CubicSplineInterpolation(t_ctrl, A_state[i,:])
            z_cubic[i] = interp_cubic(t)
        end

        z = z_rollout[end] + dt_sim * w[:,tt]
        u = eval_policy(policy, K[k], z, z_cubic, u_nom[k])

        # clip controls
        u = max.(u, ul[p])
        u = min.(u, uu[p])

        if friction
            _u = [u[1] - μ * sign(z_cubic[3]) * model.g * (model.mp + model.mc);
				  0.0;
				  0.0]
        else
            _u = u[1]
        end

        push!(z_rollout, fd(model, z, _u, zeros(model.d), dt_sim, 0))
        push!(u_rollout, u)

        if _norm == 2
            J += (z_rollout[end] - z_cubic)' * Q[k + 1] * (z_rollout[end] - z_cubic)

			J += (u_rollout[end] - u_nom[k][p])' * R[k][p,
                p] * (u_rollout[end] - u_nom[k][p])

			Jx += (z_rollout[end] - z_cubic)' * Q[k + 1] * (z_rollout[end] - z_cubic)

			Ju += (u_rollout[end] - u_nom[k][p])' * R[k][p,
                p] * (u_rollout[end] - u_nom[k][p])
        else
            J += norm(sqrt(Q[k + 1]) * (z_rollout[end] - z_cubic), _norm)
            J += norm(sqrt(R[k][p, p]) * (u - u_nom[k][p]), _norm)
            Jx += norm(sqrt(Q[k + 1]) * (z_rollout[end] - z_cubic), _norm)
            Ju += norm(sqrt(R[k][p, p]) * (u - u_nom[k][p]), _norm)
        end
    end
    return z_rollout, u_rollout, J / (T_sim - 1), Jx / (T_sim - 1), Ju / (T_sim - 1)
end

using Random
Random.seed!(1)

# Nominal trajectories
x̄_nominal, ū_nominal = unpack(z̄_nominal, prob_nominal)
x̄_friction, ū_friction = unpack(z̄_friction, prob_friction)
x̄_dpo, ū_dpo = unpack(z[prob_dpo.prob.idx.nom], prob_dpo.prob.prob.nom)

# Policies
K_nominal, P_nominal = tvlqr(model,
	x̄_nominal, [ū_nominal[t][1:1] for t = 1:T-1], h,
 	Q, [R[t][1:1, 1:1] for t = 1:T-1])
K_friction, P_friction = tvlqr(model,
	x̄_friction, [ū_friction[t][1:1] for t = 1:T-1], h,
 	Q, [R[t][1:1, 1:1] for t = 1:T-1])
θ = get_policy(z, prob_dpo)

# Simulation
T_sim = 10 * T
dt_sim = tf / (T_sim - 1)

W = Distributions.MvNormal(zeros(model.n), Diagonal(1.0e-5 * ones(model.n)))
w = rand(W, T_sim)

W0 = Distributions.MvNormal(zeros(model.n), Diagonal(1.0e-5 * ones(model.n)))
w0 = rand(W0, 1)

model_sim = CartpoleFriction{RK3, FixedTime}(4, 7, 4, 1.0, 0.2, 0.5, 9.81, 0.1)

μ_sim = 0.1

t_sim = range(0, stop = tf, length = T_sim)
z_lqr, u_lqr, J_lqr, Jx_lqr, Ju_lqr = simulate(model_sim,
	policy,
	K_nominal,
    x̄_nominal, ū_nominal,
	Q, R,
	T_sim, h,
	x1 + vec(w0),
	w,
	ul = ul_friction, uu = uu_friction,
    friction = true,
    μ = μ0)

z_lqr_fr, u_lqr_fr, J_lqr_fr, Jx_lqr_fr, Ju_lqr_fr = simulate(model_sim,
	policy,
	K_friction,
    x̄_friction, ū_friction,
	Q, R,
	T_sim, h,
	x1 + vec(w0),
	w,
	ul = ul_friction, uu = uu_friction,
    friction = true,
    μ = μ0)

z_dpo, u_dpo, J_dpo, Jx_dpo, Ju_dpo = simulate(model_sim,
	policy,
	θ,
    x̄_dpo, ū_dpo,
	Q, R,
	T_sim, h,
	x1 + vec(w0),
	w,
	ul = ul_friction, uu = uu_friction,
    friction = true,
    μ = μ0)

# objective value
J_lqr
J_lqr_fr
J_dpo

# state tracking
Jx_lqr
Jx_lqr_fr
Jx_dpo

# control tracking
Ju_lqr
Ju_lqr_fr
Ju_dpo

# Visualize
include(joinpath(pwd(), "models/visualize.jl"))
vis = Visualizer()
render(vis)
open(vis)
visualize!(vis, model_sim, [[z_lqr_fr[1] for t = 1:100]..., z_lqr_fr..., [z_lqr_fr[end] for t = 1:100]...], Δt = dt_sim, color = RGBA(0.0, 1.0, 1.0, 1.0))
visualize!(vis, model_sim, [[z_dpo[1] for t = 1:100]..., z_dpo..., [z_dpo[end] for t = 1:100]...], Δt = dt_sim, color = RGBA(1.0, 127 / 255, 0, 1.0))

z_lqr
z_dpo

max_issues_repo_name: thowell/DirectMotionPlanning


text:
@testset "auxiliar" begin

    m = @agent 2 [x1,y1]::Local
    @test_nowarn AgentBasedModels.checkIsDeclared_(m,:x1)
    @test_nowarn AgentBasedModels.checkIsDeclared_(m,[:x1])
    @test_nowarn AgentBasedModels.checkIsDeclared_(m,[:x1,:y1])
    @test_throws ErrorException AgentBasedModels.checkIsDeclared_(m,:g)
    @test_throws ErrorException AgentBasedModels.checkIsDeclared_(m,[:g])
    @test_throws ErrorException AgentBasedModels.checkIsDeclared_(m,[:x1,:g])

    @test AgentBasedModels.subs_(:(x += 3*x),:x,:y) == :(y += 3*y)
    @test AgentBasedModels.subs_(:(x += 3*x),:x,:y,update=true) == :(y += 3*x)

    m = @agent(
        0,
        l::Local,
        g::Global,
        h::GlobalArray,
        id::Identity
    )
    s = SimulationFree(m);
    p = AgentBasedModels.Program_(m,s);
    AgentBasedModels.updates_!(p,m,s)
    @test AgentBasedModels.vectorize_(m,:(l = 1), p) == :(localV[ic1_,1]=1)
    @test AgentBasedModels.vectorize_(m,:(l_i = 1), p) == :(localV[ic1_,1]=1)
    @test AgentBasedModels.vectorize_(m,:(l_j = 1), p) == :(localV[nnic2_,1]=1)
    @test AgentBasedModels.vectorize_(m,:(g = 1), p) == :(globalV[1]=1)
    @test AgentBasedModels.vectorize_(m,:(h[1,2] = 1), p) == :(h[1,2]=1)
    @test AgentBasedModels.vectorize_(m,:(id = 1), p) == :(identityV[ic1_,2]=1)
    @test AgentBasedModels.vectorize_(m,:(id_i = 1), p) == :(identityV[ic1_,2]=1)
    @test AgentBasedModels.vectorize_(m,:(id_j = 1), p) == :(identityV[nnic2_,2]=1)

    @test AgentBasedModels.vectorize_(m,:(l = l), p) == :(localV[ic1_,1]=localV[ic1_,1])
    @test AgentBasedModels.vectorize_(m,:(l_i = l_i), p) == :(localV[ic1_,1]=localV[ic1_,1])
    @test AgentBasedModels.vectorize_(m,:(l_j = l_j), p) == :(localV[nnic2_,1]=localV[nnic2_,1])
    @test AgentBasedModels.vectorize_(m,:(g = g), p) == :(globalV[1]=globalV[1])
    @test AgentBasedModels.vectorize_(m,:(h[1,2] = h[1,2]), p) == :(h[1,2]=h[1,2])
    @test AgentBasedModels.vectorize_(m,:(id = id), p) == :(identityV[ic1_,2]=identityV[ic1_,2])
    @test AgentBasedModels.vectorize_(m,:(id_i = id_i), p) == :(identityV[ic1_,2]=identityV[ic1_,2])
    @test AgentBasedModels.vectorize_(m,:(id_j = id_j), p) == :(identityV[nnic2_,2]=identityV[nnic2_,2])

    @test begin
        m = @agent(
            0,
            l::Local,
            g::Global,
            h::GlobalArray,
            id::Identity
        )

        prod([i in [:t,:N,:loc_,:glob_,:id_,:h] for i in AgentBasedModels.agentArguments_(m)])
    end

    @test AgentBasedModels.cudaAdapt_(:(sin(e^2^x))) == :(CUDA.sin(e^2^x))
    @test AgentBasedModels.cudaAdapt_(:(zeros(e^2))) == :(CUDA.zeros(e^2))

    @test begin
        code = :(x .+= 1)
        f = AgentBasedModels.wrapInFunction_(:func,code)
        f = AgentBasedModels.subs_(f,:ARGS_,:x)
        eval(quote
            $f
            y = ones(10)
            func(y)
            y == 2*ones(10)
        end)
    end

    @test begin
        code = :(x[ic1_] += 1)
        code = AgentBasedModels.simpleFirstLoop_("cpu",code)
        f = AgentBasedModels.wrapInFunction_(:func,code)
        f = AgentBasedModels.subsArguments_(f,:ARGS_,[:x,:N])
        eval(quote
            $f
            N = 10
            y = ones(N)
            func(y,N)
            y == 2*ones(N)
        end)
    end

    @test begin
        code = :(x[ic1_] += 1)
        f = AgentBasedModels.simpleFirstLoopWrapInFunction_("cpu",:func,code)
        f = AgentBasedModels.subsArguments_(f,:ARGS_,[:x,:N])
        eval(quote
            $f
            N = 10
            y = ones(N)
            func(y,N)
            y == 2*ones(N)
        end)
    end

    if CUDA.has_cuda()
        @test begin
            code = :(x[ic1_] += 1)
            code = AgentBasedModels.simpleFirstLoop_("gpu",code)
            f = AgentBasedModels.wrapInFunction_(:func,code)
            f = AgentBasedModels.subsArguments_(f,:ARGS_,[:x,:N])
            eval(quote
                $f
                N = 10
                y = CUDA.ones(N)
                CUDA.@cuda func(y,N)
                Array(y) == 2*ones(N)
            end)
        end

        @test begin
            code = :(x[ic1_] += 1)
            f = AgentBasedModels.simpleFirstLoopWrapInFunction_("gpu", :func, code)
            f = AgentBasedModels.subsArguments_(f,:ARGS_,[:x,:N])
            eval(quote
                $f
                N = 10
                y = CUDA.ones(N)
                CUDA.@cuda func(y,N)
                Array(y) == 2*ones(N)
            end)
        end
    end

    @test begin

        m = DataFrame(Symbol=Symbol[], updated=Bool[], assigned=Bool[], referenced=Bool[], called=Bool[], placeDeclaration=Symbol[], type=Symbol[])
        push!(m,(:l,false,true,false,false,:Model,:Local))
        push!(m,(:g,true,false,true,false,:Model,:GlobalArray))
        push!(m,(:h,true,false,false,false,:Model,:Localj))
        push!(m,(:+,false,false,false,true,:Math,:None))
        push!(m,(:l,false,false,false,false,:Model,:Locali))
        push!(m,(:g,false,false,false,false,:Model,:GlobalArray))
        push!(m,(:u,false,false,false,true,:NotDefined,:None))

        #println(m)

        abm = @agent 0 [l,h]::Local g::GlobalArray

        m2 = AgentBasedModels.symbols_(abm,
            quote
                l = 5
                g[1,2] -= 7
                h_j += l_i+g+u(1)
            end
        )

        #println(m2)

        m == m2
    end

    @test_nowarn begin

        abm = @agent(
            0,

            [l1,l2]::Local,
            [g1,g2]::Global,
            [id1,id2]::Identity,
            [ga1,ga2]::GlobalArray,

            UpdateLocal =
            begin
                l1 += 1
                l2 = 3
                id1 = 1
            end,

            Equation =
            begin
                d_l1 = a*dt + b*dW
            end,

            UpdateGlobal =
            begin
                g1 += 1
                ga1[1,2] += 1
            end
        )
        s= SimulationFree(abm)
        p = AgentBasedModels.Program_(abm,s);

        AgentBasedModels.updates_!(p,abm,s)

        if p.update["Identity"] != Dict(:id1=>1) error() end
        if p.update["Global"] != Dict(:g1=>1) error() end
        if p.update["GlobalArray"] != Dict(:ga1=>1) error() end
        if p.update["Local"] != Dict(:l2=>2,:l1=>1) error() end
        if p.update["Variables"] != Dict(:l1=>1) error() end
    end

    @test AgentBasedModels.extract(:(f(x)*1+4),:f) == (:x)

end

max_stars_repo_name: gatocor/embryogenesisJulia

max_issues_repo_name: gatocor/embryogenesisJulia

max_forks_repo_name: gatocor/embryogenesisJulia


text:
import Base.&

#=
&,And : Logical & of A and B
=#
function Base.&(A::Assoc, B::Assoc)
#First, create the row and col of the intersection
ABrow = intersect(A.row,B.row)
ABcol = intersect(A.col,B.col)
#Filling the sparse matrix with

AA = spzeros(size(ABrow,1), size(ABcol,1))
rowMapping = searchsortedmapping(ABrow,A.row)
colMapping = searchsortedmapping(ABcol,A.col)
AA = spones(A.A[rowMapping,colMapping])
AA = round(Int64,AA)

BB = spzeros(size(ABrow,1), size(ABcol,1))
rowMapping = searchsortedmapping(ABrow,B.row)
colMapping = searchsortedmapping(ABcol,B.col)
BB = spones(B.A[rowMapping,colMapping])
BB = round(Int64,BB)

ABA = AA & BB
ABA = ABA * 1.0

return Assoc(ABrow,ABcol,promote([1.0],A.val)[1],ABA)
end

########################################################
# D4M: Dynamic Distributed Dimensional Data Model
# Architect: Dr. Jeremy Kepner (kepner@ll.mit.edu)
# Software Engineer: Alexander Chen (alexc89@mit.edu)
########################################################


text:
export HSAError

type HSAError <: Exception
    status::hsa_status_t
end

import Base.show

function show(io::IO, e::HSAError)
    local message
    try
        message = status_string(e.status)
    catch ex
        message = sprint(show, ex)
    end

    print(io, typeof(e), "(", e.status, ") ", message)
end

status_string(status) = status_string(convert(hsa_status_t, status))

function test_status(status; warn_only :: Bool = false)
    status = convert(hsa_status_t, status)
    return test_status(status, warn_only)
end
function test_status(status::hsa_status_t; warn_only :: Bool = false)
    error = status & HSA_STATUS_ERROR == HSA_STATUS_ERROR

    if error
        ex = HSAError(status)
        if warn_only
            warn(ex)
        else
            throw(ex)
        end
    end

    return !error
end


text:
#-------------------------------------------------------------------------------
# File: greedy.jl
# Description: This file contains all function used to construct
#         a first valid solution.
# Date: October 31, 2019
# Author: Jonathan Fontaine, Killian Fretaud, Rémi Garcia,
#         Boualem Lamraoui, Benoît Le Badezet, Benoit Loger
#-------------------------------------------------------------------------------

"""
    filter_on_max_criterion(candidates::Array{Int,1},
                            criterion::Union{Array{Int,1},Array{Float64,1}})

Takes a set of candidates and removes all elements with bad criterion.
"""
function filter_on_max_criterion(candidates::Array{Int,1},
                                 criterion::Union{Array{Int,1},Array{Float64,1}})
    tmp_candidates = [candidates[1]]
    max_criterion = criterion[1]
    for i in 2:length(criterion)
        # next candidate...
        if criterion[i] > max_criterion # ...is better (keep only him)
            tmp_candidates = [candidates[i]]
            max_criterion = criterion[i]
        elseif criterion[i] == max_criterion # ...is even (keep him too)
            push!(tmp_candidates, candidates[i])
        end # ...is worse (throw it away)
    end
    return tmp_candidates
end

"""
    filter_on_min_criterion(candidates::Array{Int,1},
                            criterion::Array{Int,1})

Takes a set of candidates and removes all elements with bad criterion.
"""
function filter_on_min_criterion(candidates::Array{Int,1},
                                 criterion::Array{Int,1})
    tmp_candidates = [candidates[1]]
    min_criterion = criterion[1]
    for i in 2:length(criterion)
        # next candidate...
        if criterion[i] < min_criterion # ...is better (keep only him)
            tmp_candidates = [candidates[i]]
            min_criterion = criterion[i]
        elseif criterion[i] == min_criterion # ...is even (keep him too)
            push!(tmp_candidates, candidates[i])
        end # ...is worse (throw it away)
    end
    return tmp_candidates
end

"""
    greedy(instance::Instances)

Takes an `Instance` and return a valid `Solution`.
"""
function greedy(instance::Instance)
    # The constructive greedy heuristic starts with a partial sequence formed
    # by the remaining cars from the previous day. We compute an empty sequence
    # with some cars already scheduled
    solution = init_solution(instance)
    # We have V the set of cars to be scheduled
    len = (instance.nb_cars) - (instance.nb_late_prec_day)
    V = collect((instance.nb_late_prec_day+1):(instance.nb_cars))

    # Compute for each option the number of cars who need it in V
    # TODO : Could be done in the parser and stocked in the instance
    rv = sum(instance.RC_flag,dims=1)

    # For the nb_late_prec_day first cars, update M1, M2, M3
    update_matrices!(solution, instance)

    # Initialization for first tie-break criterion
    # Compute for each option the number of cars who need it in Pi
    rpi = zeros(Int,instance.nb_HPRC)
    for i in 1:instance.nb_late_prec_day
        for j in 1:instance.nb_HPRC
            if instance.RC_flag[i,j]
                rpi[j] = rpi[j]+1
            end
        end
    end

    # The greedy criterion consists in choosing, at each iteration, the car
    # that induces the smallest number of new violations when inserted at
    # the end of the current partial sequence.
    for position in (instance.nb_late_prec_day+1):(instance.nb_cars)
        # Compute the number of violations caused by each car
        nb_new_violation = zeros(Int, len)
        for c in 1:len
            for j in 1:instance.nb_HPRC
                if instance.RC_flag[V[c], j]
                    last_ended_sequence = (position - instance.RC_q[j])
                    if last_ended_sequence > 0
                        nb_new_violation[c] += solution.M3[j, position-1] - solution.M3[j, last_ended_sequence]
                    else
                        nb_new_violation[c] += solution.M3[j, position-1]
                    end
                end
            end
        end

        # Compute the set of indices causing minimal-violation
        candidates = filter_on_min_criterion(V, nb_new_violation)

        # Two candidates or more - First tie break
        # If the average number of cars demanding a given option in the
        # partial sequence π is lower than that in the whole set V of vehicles,
        # then these cars should be encouraged to enter in the sequence.
        if length(candidates) > 1
            # Compute the tie break criterion for each candidates
            tie_break = zeros(Int, length(candidates))
            for i in 1:length(candidates)
                for j in 1:instance.nb_HPRC
                    cond1 = !instance.RC_flag[candidates[i],j]
                    cond2 = (rv[j]-rpi[j])/len > (rpi[j])/solution.length
                    tie_break[i] += Int(xor( cond1 , cond2 ))
                end
            end

            # Compute the new candidate list
            candidates = filter_on_max_criterion(candidates, tie_break)
        end

        # Two candidates or more - Second tie break
        # Cars that require options with higher utilization rates should enter.
        if length(candidates) > 1
            # Compute the utilization rate of each options
            utilization_rate = Array{Float64,1}(UndefInitializer(),instance.nb_HPRC)
            for j in 1:instance.nb_HPRC
                utilization_rate[j] = ( (rv[j] - rpi[j])/len ) / ( instance.RC_p[j] / instance.RC_q[j] )
            end
            tie_break = zeros(length(candidates))
            for i in 1:length(candidates)
                for j in 1:instance.nb_HPRC
                    tie_break[i] += instance.RC_flag[candidates[i],j] * utilization_rate[j]
                end
            end

            # Compute the new candidate list
            candidates = filter_on_max_criterion(candidates, tie_break)
        end

        # Two candidates or more - LPRC criterion
        # Choose the car that induces the smallest number of new violations of LPRC
        # when inserted at the end of the current partial sequence.
        if length(candidates) > 1
            # Compute the number of violations caused by each car
            nb_new_violation = zeros(Int, length(candidates))
            for ind in 1:length(candidates)
                c = candidates[ind]
                for j in (instance.nb_HPRC+1):(instance.nb_HPRC+instance.nb_LPRC)
                    if instance.RC_flag[c, j]
                        last_ended_sequence = (position - instance.RC_q[j])
                        if last_ended_sequence > 0
                            nb_new_violation[ind] += solution.M3[j, position-1] - solution.M3[j, last_ended_sequence]
                        else
                            nb_new_violation[ind] += solution.M3[j, position-1]
                        end
                    end
                end
            end
            candidates = filter_on_min_criterion(candidates, nb_new_violation)
        end

        c = rand(candidates)     # We have a valid candidate
        solution.sequence[position] = c
        solution.length += 1
        len = len - 1
        filter!(x->x≠c, V)    # The car is not in the list anymore

        # The utilization rates components are dynamically updated.
        # Update rpi with the options of c
        for j in 1:instance.nb_HPRC
            rpi[j] += Int(instance.RC_flag[c,j])
        end

        # Update M1, M2 and M3
        update_matrices_new_car!(solution, position, instance)
    end

    return solution
end

"""
    greedy_pcc(instance::Instances)

Takes an `Instance` and return a valid `Solution`.
"""
function greedy_pcc(instance::Instance)
    # The constructive greedy heuristic starts with a partial sequence formed
    # by the remaining cars from the previous day. We compute an empty sequence
    # with some cars already scheduled
    solution = init_solution(instance)
    # We have V the set of cars to be scheduled
    len = (instance.nb_cars) - (instance.nb_late_prec_day)
    V = collect((instance.nb_late_prec_day+1):(instance.nb_cars))

    # Compute for each option the number of cars who need it in V
    # TODO : Could be done in the parser and stocked in the instance
    rv = sum(instance.RC_flag,dims=1)

    # For the nb_late_prec_day first cars, update M1, M2, M3
    update_matrices!(solution, instance)

    # Initialization for first tie-break criterion
    # Compute for each option the number of cars who need it in Pi
    rpi = zeros(Int,instance.nb_HPRC)
    for i in 1:instance.nb_late_prec_day
        for j in 1:instance.nb_HPRC
            if instance.RC_flag[i,j]
                rpi[j] = rpi[j]+1
            end
        end
    end

    # Get colors
    same_colors = Array{Array{Int, 1}, 1}()
    for color_groups in instance.same_color
        push!(same_colors, copy(color_groups.second))
    end
    sort!(same_colors, by = x -> length(x), rev = true)
    batch_size = 0

    # The greedy criterion consists in choosing, at each iteration, the car
    # that induces the smallest number of new violations when inserted at
    # the end of the current partial sequence.
    for position in (instance.nb_late_prec_day+1):(instance.nb_cars)
        # Compute the number of violations caused by each car
        nb_new_violation = zeros(Int, len) .+ typemax(Int)
        for c in 1:len
            if V[c] in same_colors[1]
                nb_new_violation[c] = 0
                for j in 1:instance.nb_HPRC
                    if instance.RC_flag[V[c], j]
                        last_ended_sequence = (position - instance.RC_q[j])
                        if last_ended_sequence > 0
                            nb_new_violation[c] += solution.M3[j, position-1] - solution.M3[j, last_ended_sequence]
                        else
                            nb_new_violation[c] += solution.M3[j, position-1]
                        end
                    end
                end
            end
        end

        # Compute the set of indices causing minimal-violation
        candidates = filter_on_min_criterion(V, nb_new_violation)

        # Two candidates or more - First tie break
        # If the average number of cars demanding a given option in the
        # partial sequence π is lower than that in the whole set V of vehicles,
        # then these cars should be encouraged to enter in the sequence.
        if length(candidates) > 1
            # Compute the tie break criterion for each candidates
            tie_break = zeros(Int, length(candidates))
            for i in 1:length(candidates)
                for j in 1:instance.nb_HPRC
                    cond1 = !instance.RC_flag[candidates[i],j]
                    cond2 = (rv[j]-rpi[j])/len > (rpi[j])/solution.length
                    tie_break[i] += Int(xor( cond1 , cond2 ))
                end
            end

            # Compute the new candidate list
            candidates = filter_on_max_criterion(candidates, tie_break)
        end

        # Two candidates or more - Second tie break
        # Cars that require options with higher utilization rates should enter.
        if length(candidates) > 1
            # Compute the utilization rate of each options
            utilization_rate = Array{Float64,1}(UndefInitializer(),instance.nb_HPRC)
            for j in 1:instance.nb_HPRC
                utilization_rate[j] = ( (rv[j] - rpi[j])/len ) / ( instance.RC_p[j] / instance.RC_q[j] )
            end
            tie_break = zeros(length(candidates))
            for i in 1:length(candidates)
                for j in 1:instance.nb_HPRC
                    tie_break[i] += instance.RC_flag[candidates[i],j] * utilization_rate[j]
                end
            end

            # Compute the new candidate list
            candidates = filter_on_max_criterion(candidates, tie_break)
        end

        # Two candidates or more - LPRC criterion
        # Choose the car that induces the smallest number of new violations of LPRC
        # when inserted at the end of the current partial sequence.
        if length(candidates) > 1
            # Compute the number of violations caused by each car
            nb_new_violation = zeros(Int, length(candidates))
            for ind in 1:length(candidates)
                c = candidates[ind]
                for j in (instance.nb_HPRC+1):(instance.nb_HPRC+instance.nb_LPRC)
                    if instance.RC_flag[c, j]
                        last_ended_sequence = (position - instance.RC_q[j])
                        if last_ended_sequence > 0
                            nb_new_violation[ind] += solution.M3[j, position-1] - solution.M3[j, last_ended_sequence]
                        else
                            nb_new_violation[ind] += solution.M3[j, position-1]
                        end
                    end
                end
            end
            candidates = filter_on_min_criterion(candidates, nb_new_violation)
        end

        c = rand(candidates)     # We have a valid candidate
        deleteat!(same_colors[1], findfirst(isequal(c), same_colors[1]))
        solution.sequence[position] = c
        solution.length += 1
        len = len - 1
        batch_size += 1
        if isempty(same_colors[1])
            batch_size = 0
            sort!(same_colors, by = x -> length(x), rev = true)
            pop!(same_colors)
        elseif batch_size == instance.nb_paint_limitation
            batch_size = 0
            sort!(same_colors, by = x -> length(x), rev = true)
            if instance.color_code[c] == instance.color_code[same_colors[1][1]]
                try
                    same_colors[1], same_colors[2] = copy(same_colors[2]), copy(same_colors[1])
                catch
                    position_car = instance.nb_late_prec_day+1
                    #TODO this condition might not be enough in peculiar cases
                    while instance.color_code[solution.sequence[position_car]] == instance.color_code[same_colors[1][1]]
                        position_car += 1
                    end
                    move_insertion!(solution, position_car, solution.length, instance)
                end
            end
        end
        filter!(x->x≠c, V)    # The car is not in the list anymore

        # The utilization rates components are dynamically updated.
        # Update rpi with the options of c
        for j in 1:instance.nb_HPRC
            rpi[j] += Int(instance.RC_flag[c,j])
        end

        # Update M1, M2 and M3
        update_matrices_new_car!(solution, position, instance)
    end

    @assert solution.length == instance.nb_cars

    return solution
end