text:
"""
    calc_equilibrium_distribution(mom::JuSwalbe.Macroquant; gravity=0)

Calculates the equilibrium distributions based on the macroscopic quantities `mom` and gravitational acceleration `gravity`.

The equilibrium distribtutions are at the heart of the lattice Boltzmann method.
As the expansion is made around the equilibrium, lattice Boltzmann always assumes that the flow field is close to equilibrium.
Therefore the equilibrium is calculated from macroscopic quantities, i.e. height `.height` and velocity `.velocity`.

# Math:
## One spatial dimension
Defining equations in one spatial dimensions are expressed in the following way

``f_0^{eq} =  h - \\frac{1}{2v^2}gh^2 - \\frac{1}{v^2}hu^2``

``f_1^{eq} =  \\frac{1}{4v^2}gh^2 + \\frac{1}{2v}hu + \\frac{1}{2v^2}hu^2``

``f_2^{eq} =  \\frac{1}{4v^2}gh^2 - \\frac{1}{2v}hu + \\frac{1}{2v^2}hu^2 ``

## Two spatial dimension
In two spatial dimensions the velocity because a vector with a `x` and `y` component.
Such the equations look a little more complex

``f_0^{eq} =  h - \\frac{4}{9}h(\\frac{15}{2}gh - \\frac{3}{2}u^2)``

``f_i^{eq} = w_i h(\\frac{3}{2}gh + 3 \\mathbf{c}_i\\cdot\\mathbf{u} + \\frac{9}{2}(\\mathbf{c}_i\\cdot\\mathbf{u})^2 - \\frac{3}{2}u^2))``

with i are the number of lattice speeds and w_i and c_i are the weights and sets of lattice velocities.

# Example
## Two spatial dimensions
```jldoctest
julia> using JuSwalbe

julia> mom = JuSwalbe.Macroquant(height=ones(4,4), velocity=JuSwalbe.Twovector(x=fill(0.1, (4,4)), y=fill(-0.1, (4,4))), pressure=zeros(4,4), energy=zeros(4,4))
JuSwalbe.Macroquant{Array{Float64,2},JuSwalbe.Twovector{Array{Float64,2}}}
  height: Array{Float64}((4, 4)) [1.0 1.0 1.0 1.0; 1.0 1.0 1.0 1.0; 1.0 1.0 1.0 1.0; 1.0 1.0 1.0 1.0]
  velocity: JuSwalbe.Twovector{Array{Float64,2}}
  pressure: Array{Float64}((4, 4)) [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.0 0.0]
  energy: Array{Float64}((4, 4)) [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.0 0.0]

julia> equilibrium = calc_equilibrium_distribution(mom)
JuSwalbe.DistributionD2Q9{Array{Float64,2}}
  f0: Array{Float64}((4, 4)) [0.9866666666666667 0.9866666666666667 0.9866666666666667 0.9866666666666667; 0.9866666666666667 0.9866666666666667 0.9866666666666667 0.9866666666666667; 0.9866666666666667 0.9866666666666667 0.9866666666666667 0.9866666666666667; 0.9866666666666667 0.9866666666666667 0.9866666666666667 0.9866666666666667]
  f1: Array{Float64}((4, 4)) [0.035 0.035 0.035 0.035; 0.035 0.035 0.035 0.035; 0.035 0.035 0.035 0.035; 0.035 0.035 0.035 0.035]
  f2: Array{Float64}((4, 4)) [-0.03166666666666667 -0.03166666666666667 -0.03166666666666667 -0.03166666666666667; -0.03166666666666667 -0.03166666666666667 -0.03166666666666667 -0.03166666666666667; -0.03166666666666667 -0.03166666666666667 -0.03166666666666667 -0.03166666666666667; -0.03166666666666667 -0.03166666666666667 -0.03166666666666667 -0.03166666666666667]
  f3: Array{Float64}((4, 4)) [-0.03166666666666667 -0.03166666666666667 -0.03166666666666667 -0.03166666666666667; -0.03166666666666667 -0.03166666666666667 -0.03166666666666667 -0.03166666666666667; -0.03166666666666667 -0.03166666666666667 -0.03166666666666667 -0.03166666666666667; -0.03166666666666667 -0.03166666666666667 -0.03166666666666667 -0.03166666666666667]
  f4: Array{Float64}((4, 4)) [0.035 0.035 0.035 0.035; 0.035 0.035 0.035 0.035; 0.035 0.035 0.035 0.035; 0.035 0.035 0.035 0.035]
  f5: Array{Float64}((4, 4)) [-0.0008333333333333335 -0.0008333333333333335 -0.0008333333333333335 -0.0008333333333333335; -0.0008333333333333335 -0.0008333333333333335 -0.0008333333333333335 -0.0008333333333333335; -0.0008333333333333335 -0.0008333333333333335 -0.0008333333333333335 -0.0008333333333333335; -0.0008333333333333335 -0.0008333333333333335 -0.0008333333333333335 -0.0008333333333333335]
  f6: Array{Float64}((4, 4)) [-0.0125 -0.0125 -0.0125 -0.0125; -0.0125 -0.0125 -0.0125 -0.0125; -0.0125 -0.0125 -0.0125 -0.0125; -0.0125 -0.0125 -0.0125 -0.0125]
  f7: Array{Float64}((4, 4)) [-0.0008333333333333335 -0.0008333333333333335 -0.0008333333333333335 -0.0008333333333333335; -0.0008333333333333335 -0.0008333333333333335 -0.0008333333333333335 -0.0008333333333333335; -0.0008333333333333335 -0.0008333333333333335 -0.0008333333333333335 -0.0008333333333333335; -0.0008333333333333335 -0.0008333333333333335 -0.0008333333333333335 -0.0008333333333333335]
  f8: Array{Float64}((4, 4)) [0.020833333333333336 0.020833333333333336 0.020833333333333336 0.020833333333333336; 0.020833333333333336 0.020833333333333336 0.020833333333333336 0.020833333333333336; 0.020833333333333336 0.020833333333333336 0.020833333333333336 0.020833333333333336; 0.020833333333333336 0.020833333333333336 0.020833333333333336 0.020833333333333336]

julia> arr = dist2array(equilibrium)
9×4×4 Array{Float64,3}:
[:, :, 1] =
  0.986667      0.986667      0.986667      0.986667
  0.035         0.035         0.035         0.035
 -0.0316667    -0.0316667    -0.0316667    -0.0316667
 -0.0316667    -0.0316667    -0.0316667    -0.0316667
  0.035         0.035         0.035         0.035
 -0.000833333  -0.000833333  -0.000833333  -0.000833333
 -0.0125       -0.0125       -0.0125       -0.0125
 -0.000833333  -0.000833333  -0.000833333  -0.000833333
  0.0208333     0.0208333     0.0208333     0.0208333

[:, :, 2] =
  0.986667      0.986667      0.986667      0.986667
  0.035         0.035         0.035         0.035
 -0.0316667    -0.0316667    -0.0316667    -0.0316667
 -0.0316667    -0.0316667    -0.0316667    -0.0316667
  0.035         0.035         0.035         0.035
 -0.000833333  -0.000833333  -0.000833333  -0.000833333
 -0.0125       -0.0125       -0.0125       -0.0125
 -0.000833333  -0.000833333  -0.000833333  -0.000833333
  0.0208333     0.0208333     0.0208333     0.0208333

[:, :, 3] =
  0.986667      0.986667      0.986667      0.986667
  0.035         0.035         0.035         0.035
 -0.0316667    -0.0316667    -0.0316667    -0.0316667
 -0.0316667    -0.0316667    -0.0316667    -0.0316667
  0.035         0.035         0.035         0.035
 -0.000833333  -0.000833333  -0.000833333  -0.000833333
 -0.0125       -0.0125       -0.0125       -0.0125
 -0.000833333  -0.000833333  -0.000833333  -0.000833333
  0.0208333     0.0208333     0.0208333     0.0208333

[:, :, 4] =
  0.986667      0.986667      0.986667      0.986667
  0.035         0.035         0.035         0.035
 -0.0316667    -0.0316667    -0.0316667    -0.0316667
 -0.0316667    -0.0316667    -0.0316667    -0.0316667
  0.035         0.035         0.035         0.035
 -0.000833333  -0.000833333  -0.000833333  -0.000833333
 -0.0125       -0.0125       -0.0125       -0.0125
 -0.000833333  -0.000833333  -0.000833333  -0.000833333
  0.0208333     0.0208333     0.0208333     0.0208333

julia> h = sum(arr, dims=1)[1,:,:] # Check consistency
4×4 Array{Float64,2}:
 1.0  1.0  1.0  1.0
 1.0  1.0  1.0  1.0
 1.0  1.0  1.0  1.0
 1.0  1.0  1.0  1.0

```

## One spatial dimension
```jldoctest
julia> using JuSwalbe

julia> mom = JuSwalbe.Macroquant(height=ones(5), velocity=fill(0.1, 5), pressure=zeros(5), energy=zeros(5))
JuSwalbe.Macroquant{Array{Float64,1},Array{Float64,1}}
  height: Array{Float64}((5,)) [1.0, 1.0, 1.0, 1.0, 1.0]
  velocity: Array{Float64}((5,)) [0.1, 0.1, 0.1, 0.1, 0.1]
  pressure: Array{Float64}((5,)) [0.0, 0.0, 0.0, 0.0, 0.0]
  energy: Array{Float64}((5,)) [0.0, 0.0, 0.0, 0.0, 0.0]

julia> equilibrium = calc_equilibrium_distribution(mom)
JuSwalbe.DistributionD1Q3{Array{Float64,1}}
  f0: Array{Float64}((5,)) [0.99, 0.99, 0.99, 0.99, 0.99]
  f1: Array{Float64}((5,)) [0.05500000000000001, 0.05500000000000001, 0.05500000000000001, 0.05500000000000001, 0.05500000000000001]
  f2: Array{Float64}((5,)) [-0.045, -0.045, -0.045, -0.045, -0.045]

julia> h = sum(hcat(equilibrium.f0, equilibrium.f1, equilibrium.f2), dims=2)[:,1]
5-element Array{Float64,1}:
 0.9999999999999999
 0.9999999999999999
 0.9999999999999999
 0.9999999999999999
 0.9999999999999999

julia> h[1] - 1.0 # fits up to machine precision
-1.1102230246251565e-16

```

# References
## Code
See also: [`velocitysquared`](@ref)

## One spatial dimension
There are plenty of ways to calculate them I mainly the ones derived in Eq.(13) of [Study of the 1D lattice Boltzmann shallow water equation and its coupling to build a canal network.](https://www.sciencedirect.com/science/article/pii/S0021999110003372)

## Two spatial dimensions
Here I stick to the paper written by Paul Dellar, Eq.(26) of [Nonhydrodynamic modes and *a priori* construction of shallow water lattice Boltzmann equations](https://journals.aps.org/pre/abstract/10.1103/PhysRevE.65.036309)
Originally these equilibria have been worked out by Paul Salmon.
"""
function calc_equilibrium_distribution(mom::JuSwalbe.Macroquant{Vector{T}, Vector{T}}; gravity::T=T(0)) where {T<:Number}
    len = length(mom.height)
    v = T(1.0)
    vsquare = v^2
    equi_dist = JuSwalbe.DistributionD1Q3(f0=zeros(T, len), f1=zeros(T, len), f2=zeros(T, len))
    equi_dist.f0 = (mom.height .- 1/(2*vsquare) * gravity .* mom.height.^2
                    .- 1/vsquare .* mom.height .* mom.velocity.^2)
    equi_dist.f1 = (1/(4*vsquare) * gravity .* mom.height.^2
                    .+ 1/(2*v) .* mom.height .* mom.velocity
                    .+ 1/(2*vsquare) .* mom.height .* mom.velocity.^2)
    equi_dist.f2 = (1/(4*vsquare) * gravity .* mom.height.^2
                    .- 1/(2*v) .* mom.height .* mom.velocity
                    .+ 1/(2*vsquare) .* mom.height .* mom.velocity.^2)

    return equi_dist
end

function calc_equilibrium_distribution(mom::JuSwalbe.Macroquant{Matrix{T}, JuSwalbe.Twovector{Matrix{T}}}; gravity::T=T(0)) where {T<:Number}
    # In two dimensions there is a little more work needed
    width = size(mom.height,1)
    thick = size(mom.height,2)

    v = 1.0
    vsquare = v^2
    # Standard D2Q9 weights and lattice velocities
    # For type security, otherwise weights will always be Float64
    weights = [T(4/9), T(1/9), T(1/9), T(1/9), T(1/9), T(1/36), T(1/36), T(1/36), T(1/36)]

    lattice_vel = [0 0; 1 0; 0 1; -1 0; 0 -1; 1 1; -1 1; -1 -1; 1 -1]

    # Initialize a dummy distribution
    equi_dist = JuSwalbe.DistributionD2Q9(f0=zeros(T,(width, thick)),
                                          f1=zeros(T,(width, thick)),
                                          f2=zeros(T,(width, thick)),
                                          f3=zeros(T,(width, thick)),
                                          f4=zeros(T,(width, thick)),
                                          f5=zeros(T,(width, thick)),
                                          f6=zeros(T,(width, thick)),
                                          f7=zeros(T,(width, thick)),
                                          f8=zeros(T,(width, thick)))
    # Calculate the velocity squared
    udotu = velocitysquared(mom)
    # Again make use dicts for iteration purpose
    equiarray = dist2array(equi_dist)
    # Calculate f0
    equiarray[1, :, :] .= mom.height .+ weights[1] * mom.height .*(T(-15.0/8.0) * gravity * mom.height .- T(3.0/2.0) * udotu)
    # And the other 8 distribtuion functions
    for i in 2:9
        latticeveldotu = mom.velocity.x * lattice_vel[i,1] .+ mom.velocity.y * lattice_vel[i,2]
        equiarray[i, :, :] .= weights[i] * mom.height .* (T(3.0/2.0) * gravity * mom.height
                                                          .+ T(3) * latticeveldotu
                                                          .+ T(9.0/2.0) * latticeveldotu.^2
                                                          .- T(3.0/2.0) * udotu)
    end
    # Save them to the distrubtion type which was created earlier
    equi_dist = JuSwalbe.DistributionD2Q9(f0=equiarray[1, :, :],
                                          f1=equiarray[2, :, :],
                                          f2=equiarray[3, :, :],
                                          f3=equiarray[4, :, :],
                                          f4=equiarray[5, :, :],
                                          f5=equiarray[6, :, :],
                                          f6=equiarray[7, :, :],
                                          f7=equiarray[8, :, :],
                                          f8=equiarray[9, :, :])

    # And return it :)
    return equi_dist
end

"""
    velocitysquared(mom::Macroquant{Matrix{T}, JuSwalbe.Twovector{Matrix{T}}})

Computes the square of the velocity vector (ux, uy) at every lattice point.

The magnitude of the velocity is needed to calculate the equilibrium distribution in the dimensional case.
In the one dimensional case the velocity is just a vector and therefore has no `.x` and `.y` component.
See also [`calc_equilibrium_distribution`](@ref)

# Math
The velocity squared ``u^2(x,y)`` is computed according to

`` u^2(x,y) = (u_x, u_y)^2(x,y) = u_x^2(x,y) + u_y^2(x,y).``

With lower case `x` and `y` the respective component of the velocity vector is addressed.

# Example
```jldoctest
julia> using JuSwalbe

julia> velocities = JuSwalbe.Twovector{Matrix{Float64}}(x=fill(0.1, (4,4)), y=fill(0.2, (4,4)))
JuSwalbe.Twovector{Array{Float64,2}}
  x: Array{Float64}((4, 4)) [0.1 0.1 0.1 0.1; 0.1 0.1 0.1 0.1; 0.1 0.1 0.1 0.1; 0.1 0.1 0.1 0.1]
  y: Array{Float64}((4, 4)) [0.2 0.2 0.2 0.2; 0.2 0.2 0.2 0.2; 0.2 0.2 0.2 0.2; 0.2 0.2 0.2 0.2]

julia> moment = JuSwalbe.Macroquant(height=ones(Float64, (4,4)), velocity=velocities, pressure=ones(Float64, (4,4)), energy=ones(Float64, (4,4)))
JuSwalbe.Macroquant{Array{Float64,2},JuSwalbe.Twovector{Array{Float64,2}}}
  height: Array{Float64}((4, 4)) [1.0 1.0 1.0 1.0; 1.0 1.0 1.0 1.0; 1.0 1.0 1.0 1.0; 1.0 1.0 1.0 1.0]
  velocity: JuSwalbe.Twovector{Array{Float64,2}}
  pressure: Array{Float64}((4, 4)) [1.0 1.0 1.0 1.0; 1.0 1.0 1.0 1.0; 1.0 1.0 1.0 1.0; 1.0 1.0 1.0 1.0]
  energy: Array{Float64}((4, 4)) [1.0 1.0 1.0 1.0; 1.0 1.0 1.0 1.0; 1.0 1.0 1.0 1.0; 1.0 1.0 1.0 1.0]

julia> moment.velocity.x
4×4 Array{Float64,2}:
 0.1  0.1  0.1  0.1
 0.1  0.1  0.1  0.1
 0.1  0.1  0.1  0.1
 0.1  0.1  0.1  0.1

julia> JuSwalbe.velocitysquared(moment)
4×4 Array{Float64,2}:
 0.05  0.05  0.05  0.05
 0.05  0.05  0.05  0.05
 0.05  0.05  0.05  0.05
 0.05  0.05  0.05  0.05
```
"""
function velocitysquared(mom::Macroquant{Matrix{T}, JuSwalbe.Twovector{Matrix{T}}}) where {T<:Number}
    velsquared = mom.velocity.x.^2 .+ mom.velocity.y.^2
    return velsquared
end

max_stars_repo_name: Zitzeronion/JuSwalbe

max_issues_repo_name: Zitzeronion/JuSwalbe


text:
function forsub(A::AbstractArray{T,2}, b::AbstractVector{T}) where {T<:Number}
#
# assume A is an lower triangular matrix
# forsub performs forward substitution to solve Ax=b
#
    n = size(A,1)
    LowerTriangular(A) \ b[1:n]
end

ext: jl


text:
struct Satellite <: LieGroupModel
    J::Diagonal{Float64,SVector{3,Float64}}
end

Satellite() = Satellite(Diagonal(@SVector ones(3)))

RobotDynamics.control_dim(::Satellite) = 3
Base.position(::Satellite, x::SVector) = @SVector zeros(3)
RobotDynamics.orientation(::Satellite, x::SVector) = UnitQuaternion(x[4], x[5], x[6], x[7])

RobotDynamics.LieState(::Satellite) = RobotDynamics.LieState(UnitQuaternion{Float64}, (3,0))

function RobotDynamics.dynamics(model::Satellite, x::SVector, u::SVector)
    ω = @SVector [x[1], x[2], x[3]]
    q = normalize(@SVector [x[4], x[5], x[6], x[7]])
    J = model.J

    ωdot = J\(u - ω × (J*ω))
    qdot = 0.5*lmult(q)*hmat()*ω
    return [ωdot; qdot]
end

model = Satellite()
@test LieState(model) === RobotDynamics.QuatState(7, (4,))
@test state_dim(model) == 7
@test state_diff_size(model) == 6

x,u = rand(model)
s = LieState(model)
@test all(RobotDynamics.vec_states(s, x) .≈ (x[1:3],Float64[]))
@test all(RobotDynamics.rot_states(s, x) .≈ (UnitQuaternion(x[4:7]),))
@test all(RobotDynamics.vec_states(model, x) .≈ (x[1:3],Float64[]))
@test all(RobotDynamics.rot_states(model, x) .≈ (UnitQuaternion(x[4:7]),))

x2 = rand(s)
@test norm(x[4:7]) ≈ 1

dx = RobotDynamics.state_diff(model, x, x2)
@test length(dx) == 6
@test dx ≈ [x[1:3] - x2[1:3]; UnitQuaternion(x[4:7]) ⊖ UnitQuaternion(x2[4:7])]


text:
######################
# Redundancy removal #
######################

# Redundancy
export isredundant, removevredundancy!, removevredundancy, removehredundancy!, removehredundancy, Redundancy, vredundancy, hredundancy

"""
    @enum Redundancy UNKNOWN_REDUNDANCY LINEARITY_DETECTED NO_REDUNDANCY

Redundancy of a H-representation or V-representation.

* `UNKNOWN_REDUNDANCY`: It is unknown whether there are any undetected linearity or redundancy.
* `LINEARITY_DETECTED`: There are no undetected linearity.
* `NO_REDUNDANCY`: There are no undetected linearity not redundancy.

An *undetected linearity* for a V-representation is a [`Line`](@ref) that is implicit in a conic hull of [`Ray`](@ref)s.
For instance, `conichull([1, 1], [-1, -1])` has undetected linearity because it contains
the [`Line`](@ref) `Line([1, 1])`.
Some undetected linearity is less obvious, e.g., `conichull([1, 0, -1], [0, 1, 1], [-1, -1, 0])`
contains the [`Line`](@ref) `Line([1, 1, 0])` as the sum of the first two [`Ray`](@ref)s is `Ray([1, 1, 0])`.

An *undetected linearity* for a H-representation is a [`HyperPlane`](@ref) that is implicit in an intersection of [`HalfSpace`](@ref)s.
For instance, `HalfSpace([1, 1], 1) ∩ HalfSpace([-1, -1], 1)` has undetected linearity because it contains
the [`HyperPlane`](@ref) `HyperPlane([1, 1], 1)`.
Some undetected linearity is less obvious, e.g., `HalfSpace([1, 0], -1) ∩ HalfSpace([0, 1], 1) ∩ HalfSpace([-1, -1], 0)`
contains the [`HyperPlane`](@ref) `HyperPlane([1, 1], 0)` as the sum of the first two [`HalfSpace`](@ref)s is `HalfSpace([1, 1], 0)`.
"""
@enum Redundancy UNKNOWN_REDUNDANCY LINEARITY_DETECTED NO_REDUNDANCY

"""
    hredundancy(p::Polyhedron)

Return the [`Redundancy`](@ref) of [`hrep(p)`](@ref hrep).
"""
hredundancy(::Polyhedron) = UNKNOWN_REDUNDANCY

"""
    vredundancy(p::Polyhedron)

Return the [`Redundancy`](@ref) of [`vrep(p)`](@ref vrep).
"""
vredundancy(::Polyhedron) = UNKNOWN_REDUNDANCY

"""
    isredundant(p::Rep, idx::Index; strongly=false)

Return a `Bool` indicating whether the element with index `idx` can be removed without changing the polyhedron represented by `p`.
If `strongly` is `true`,
* if `idx` is an H-representation element `h`, it returns `true` only if no V-representation element of `p` is in the hyperplane of `h`.
* if `idx` is a V-representation element `v`, it returns `true` only if `v` is in the relative interior of `p`.
"""
function isredundant end

# `isredundant(::Polyhedron, ::HIndex)` would cause ambiguity with LRSLib
# which defines `isredundant(::LRSLib.Polyhedron, ::Index)` so we add a
# `_fallback` layer to circumvent this.
function isredundant(p::Polyhedron, idx::Index; kws...)
    return isredundant_fallback(p, idx; kws...)
end

function isredundant_fallback(p::Polyhedron, idx::HIndex; kws...)
    hredundancy(p) == NO_REDUNDANCY && return false
    solver = _solver_warn(p, false, false)
    if solver !== nothing
        @warn("""
`isredundant` with a solver is not supported yet so it triggers the computation of the V-representation, which
is computationally demanding because no solver was provided to the library.
If this is expected, call `computevrep!` explicitely before calling this
function to remove this warning.
""")
    end
    # `detecthlinearity(p)` might remove redundancy but we get `h` first here.
    h = get(p, idx)
    detectvlinearity!(p)
    detecthlinearity!(p)
    return isredundant(vrep(p), h; d = dim(p), kws...)
end

function isredundant_fallback(p::Polyhedron, idx::VIndex; strongly=false, kws...)
    vredundancy(p) == NO_REDUNDANCY && return false
    solver = _solver_warn(p, true, strongly)
    if solver !== nothing
        @warn("""
`isredundant` with a solver is not supported yet so it triggers the computation of the H-representation, which
is computationally demanding because no solver was provided to the library.
If this is expected, call `computehrep!` explicitely before calling this
function to remove this warning.
""")
    end
    # `detectvlinearity(p)` might remove redundancy but we get `v` first here.
    v = get(p, idx)
    detecthlinearity!(p)
    detectvlinearity!(p)
    return isredundant(hrep(p), h; strongly=strongly, kws...)
end

function _solver_warn(p::Polyhedron, v_or_h::Bool, strongly::Bool)
    computed = v_or_h ? hrepiscomputed(p) : vrepiscomputed(p)
    v = v_or_h ? 'v' : 'h'
    V = uppercase(v)
    h = v_or_h ? 'h' : 'v'
    solver = nothing
    if !strongly && !computed && supportssolver(typeof(p))
        solver = default_solver(p)
        if solver === nothing
            @warn("""
`remove$(v)redundancy!` will trigger the computation of the $V-representation, which
is computationally demanding because no solver was provided to the library.
If this is expected, call `compute$(h)rep!` explicitely before calling this
function to remove this warning.
""" * NO_SOLVER_HELP)
        end
    end
    return solver

end

"""
    removehredundancy!(p::HRep; kws...)

Removes the elements of the H-representation of `p` that can be removed without changing the polyhedron represented by `p`. That is, it only keeps the halfspaces corresponding to facets of the polyhedron.

The remaining keyword arguments `kws` are passed to [`detectvlinearity!`](@ref) and [`detecthlinearity!`](@ref).
"""
function removehredundancy! end
function removehredundancy!(p::Polyhedron; kws...)
    hredundancy(p) == NO_REDUNDANCY && return
    solver = _solver_warn(p, false, false)
    if solver === nothing
        detectvlinearity!(p; kws...)
        detecthlinearity!(p; kws...)
        nonred = removehredundancy(hrep(p), vrep(p))
    else
        detecthlinearity!(p; kws...)
        nonred = removehredundancy(hrep(p), solver)
    end
    sethrep!(p, nonred, NO_REDUNDANCY)
end

"""
    removevredundancy!(p::VRep; strongly=false, planar=true, kws...)

Removes the elements of the V-representation of `p` that can be removed without
changing the polyhedron represented by `p`. That is, it only keeps the extreme
points and rays. This operation is often called "convex hull" as the remaining
points are the extreme points of the convex hull of the initial set of points.
If `strongly=true`, weakly redundant points, i.e., points that are not extreme
but are not in the relative interior either, may be kept.
If `fulldim(p)` is 2, `strongly` is `false` and `planar` is `true`, a planar convex hull algorithm
is used.

The remaining keyword arguments `kws` are passed to [`detectvlinearity!`](@ref) and [`detecthlinearity!`](@ref).
"""
function removevredundancy! end
function removevredundancy!(p::Polyhedron; strongly=false, planar=true, kws...)
    vredundancy(p) == NO_REDUNDANCY && return
    if fulldim(p) == 2 && !strongly && planar
        setvrep!(p, planar_hull(vrep(p)), NO_REDUNDANCY)
    else
        solver = _solver_warn(p, true, strongly)
        if solver === nothing
            detecthlinearity!(p; kws...)
            detectvlinearity!(p; kws...)
            nonred = removevredundancy(vrep(p), hrep(p), strongly=strongly)
        else
            detectvlinearity!(p; kws...)
            nonred = removevredundancy(vrep(p), solver)
        end
        # If `strongly` then we only remove strongly redundant elements
        # henwe we cannot say that the redundancy is `NO_REDUNDANCY`.
        setvrep!(p, nonred, strongly ? LINEARITY_DETECTED : NO_REDUNDANCY)
    end
end

# Interpolating the string can take more time than the LP solve:
# https://github.com/JuliaPolyhedra/Polyhedra.jl/issues/279
# This `struct` allows to interpolate it lazily only when an error is actually thrown.
struct _RedundantMessage{ET, IT}
    element::ET
    index::IT
end
function Base.show(io::IO, message::_RedundantMessage)
    print(io, "attempting to determine whether `")
    print(io, message.element)
    print(io, "` of index `")
    print(io, message.index)
    print(io, "` is redundant.")
end

function _redundant_indices(rep::Representation, model::MOI.ModelLike, T::Type,
                            hull, indices, clean)
    red_indices = Int[]
    isempty(indices) && return red_indices
    λ, cλ, sum_one = _hull(model, T, hull, rep, indices)
    hull_con = MOI.add_constraint.(model, hull, MOI.EqualTo(zero(T)))
    for (i, idx) in enumerate(indices)
        if cλ === nothing
            fix_con = MOI.add_constraint(model, λ[i], MOI.EqualTo(zero(T)))
        else
            fix_con = MOI.transform(model, cλ[i], MOI.EqualTo(zero(T)))
        end
        element = get(rep, idx)
        for (ci, ai) in zip(hull_con, _coord_for_hull(element))
            MOI.set(model, MOI.ConstraintSet(), ci, MOI.EqualTo{T}(ai))
        end
        #if !isone(length(indices)) &&
        if  is_feasible(model, _RedundantMessage(element, idx))
            MOI.delete(model, λ[i])
            push!(red_indices, i)
        else
            if cλ === nothing
                MOI.delete(model, fix_con)
            else
                cλ[i] = MOI.transform(model, fix_con, MOI.GreaterThan(zero(T)))
            end
        end
    end
    if clean
        MOI.delete(model, hull_con)
        if !isempty(red_indices)
            rm = Set(λ[red_indices])
            hull = map(h -> MOI.Utilities.filter_variables(vi -> !(vi in rm), h), hull)
        end
    end
    return red_indices
end

function elements_without(elements, rm)
    if isempty(rm)
        return elements
    else
        return deleteat!(collect(elements), rm)
    end
end
function _nonredundant(rep, model, T, hull, elements, clean)
    red = _redundant_indices(rep, model, T, hull, eachindex(elements), clean)
    return (elements_without(elements, red),)
end

function nonredundant_hyperplanes(vr::HRepresentation, model, T, hull, clean)
    return _nonredundant(vr, model, T, hull, hyperplanes(vr), clean)
end
nonredundant_halfspaces(vr::HAffineSpace, model, T, hull, clean) = tuple()
function nonredundant_halfspaces(vr::HRepresentation, model, T, hull, clean)
    return _nonredundant(vr, model, T, hull, halfspaces(vr), clean)
end

"""
    removehredundancy(hr::HRepresentation, solver)

Return a H-representation of the polyhedron represented by `hr` with all the
elements of `hr` except the redundant ones, i.e. the elements that can
be expressed as convex combination of other ones.
"""
function removehredundancy(hr::HRep, solver)
    model, T = layered_optimizer(solver)
    hull = _zero_hull(hr, T)
    # TODO It's much more efficient to remove redundant hyperplanes with `removeduplicates`.
    #      Moreover, linearity may have already been detected.
    #      The only advantage over `removeduplicates` is that it does not alter
    #      the hyperplanes, it just remove the redundant ones.
    nr_hyperplanes = nonredundant_hyperplanes(hr, model, T, hull, true)
    nr_halfspaces = nonredundant_halfspaces(hr, model, T, hull, false)
    return hrep(nr_hyperplanes..., nr_halfspaces...; d=FullDim(hr))
end

nonredundant_lines(vr::VPolytope, model, T, hull, clean) = tuple()
function nonredundant_lines(vr::VRepresentation, model, T, hull, clean)
    return _nonredundant(vr, model, T, hull, lines(vr), clean)
end
nonredundant_rays(vr::VLinearSpace, model, T, hull, clean) = tuple()
nonredundant_rays(vr::VPolytope, model, T, hull, clean) = return tuple()
function nonredundant_rays(vr::VRepresentation, model, T, hull, clean)
    return _nonredundant(vr, model, T, hull, rays(vr), clean)
end
nonredundant_points(vr::VCone, model, T, hull, clean) = tuple()
function nonredundant_points(vr::VRepresentation, model, T, hull, clean)
    return _nonredundant(vr, model, T, hull, points(vr), clean)
end

removevredundancy(vr::VEmptySpace, solver) = vr

"""
    removevredundancy(vr::VRepresentation, solver)

Return a V-representation of the polyhedron represented by `vr` with all the
elements of `vr` except the redundant ones, i.e. the elements that can
be expressed as convex combination of other ones.
"""
function removevredundancy(vr::VRepresentation, solver)
    model, T = layered_optimizer(solver)
    hull = _zero_hull(vr, T)
    # TODO It's much more efficient to remove redundant lines with `removeduplicates`.
    #      Moreover, linearity may have already been detected.
    #      The only advantage over `removeduplicates` is that it does not alter
    #      the lines, it just remove the redundant ones.
    nr_lines = nonredundant_lines(vr, model, T, hull, true)
    nr_rays = nonredundant_rays(vr, model, T, hull, true)
    nr_points = nonredundant_points(vr, model, T, hull, false)
    return vrep(nr_points..., nr_lines..., nr_rays...; d=FullDim(vr))
end

_dim_for_hull(h::HRepresentation) = fulldim(h) + 1
_dim_for_hull(v::VRepresentation) = fulldim(v)
function _zero_hull(rep::Representation, T::Type)
    return [zero(MOI.ScalarAffineFunction{T}) for i in 1:_dim_for_hull(rep)]
end
_coord_for_hull(h::HRepElement) = [h.a; h.β]
_coord_for_hull(r::VRepElement) = coord(r)
function _fill_hull(hull::Vector{MOI.ScalarAffineFunction{T}}, λ, rep, idxs) where T
    for (λ, idx) in zip(λ, idxs)
        a = _coord_for_hull(get(rep, idx))
        for i in eachindex(a)
            push!(hull[i].terms, MOI.ScalarAffineTerm{T}(a[i], λ))
        end
    end
end
function _hull(model::MOI.ModelLike, ::Type{T}, hull::Vector{MOI.ScalarAffineFunction{T}}, rep, idxs, sum_one = idxs isa PointIndices) where T
    λ = MOI.add_variables(model, length(idxs))
    if !(idxs isa Union{HyperPlaneIndices, LineIndices})
        cλ = [MOI.add_constraint(model, λ, MOI.GreaterThan(zero(T))) for λ in λ]
    else
        cλ = nothing
    end
    if sum_one
        sum_func = MOI.ScalarAffineFunction(
            MOI.ScalarAffineTerm.(one(T), λ),
            zero(T)
        )
        sum_con = MOI.add_constraint(model, sum_func, MOI.EqualTo(one(T)))
    else
        sum_con = nothing
    end
    _fill_hull(hull, λ, rep, idxs)
    return λ, cλ, sum_con
end

function _filter(f, it)
    # FIXME returns a Vector{Any}
    #collect(Iterators.filter(f, it)) # filter does not implement length so we need to collect
    ret = eltype(it)[]
    for el in it
        if f(el)
            push!(ret, el)
        end
    end
    ret
end
function removevredundancy(vrepit::VIt, hrep::HRep; nl=nlines(hrep), kws...)
    _filter(v -> !isredundant(hrep, v; nl=nl, kws...), vrepit)
end

# Remove redundancy in the V-representation using the H-representation
# There shouldn't be any duplicates in hrep for this to work
function _removevred_withhred(vrep::VRep, hrep::HRep; kws...)
    nl = nlines(vrep)
    typeof(vrep)(
        FullDim(vrep),
        removevredundancy.(vreps(vrep), hrep; nl=nl, kws...)...
    )::typeof(vrep)
end
# Split in two methods to avoid ambiguity with `(::VRepresentation, solver::Any)`
function removevredundancy(vrep::Polyhedron, hrep::HRep; kws...)
    _removevred_withhred(vrep, hrep; kws...)
end
function removevredundancy(vrep::VRepresentation, hrep::HRep; kws...)
    _removevred_withhred(vrep, hrep; kws...)
end

function removehredundancy(hrepit::HIt, vrep::VRep; strongly=false, d=dim(vrep))
    _filter(h -> !isredundant(vrep, h, strongly=strongly, d=d), hrepit)
end

# Remove redundancy in the H-representation using the V-representation
# There shouldn't be any duplicates in vrep for this to work
function _removehred_withvred(hrep::HRep, vrep::VRep; strongly=false)
    R = BitSet()
    d = dim(hrep, true) # TODO dim(hrep)
    typeof(hrep)(FullDim(hrep),
                 removehredundancy.(hreps(hrep), vrep,
                                    strongly=strongly, d=d)...)
end
# Split in two methods to avoid ambiguity with `(::HRepresentation, solver::Any)`
function removehredundancy(hrep::Polyhedron, vrep::VRep; kws...)
    _removehred_withvred(hrep, vrep; kws...)
end
function removehredundancy(hrep::HRepresentation, vrep::VRep; kws...)
    _removehred_withvred(hrep, vrep; kws...)
end

function coord_matrix!(A, elements, condition, offset)
    for el in elements
        if condition(el)
            offset += 1
            A[offset, :] = coord(el)
        end
    end
    return offset
end

# V-redundancy
# If p is an H-representation, nl needs to be given otherwise if p is a Polyhedron, it can be asked to p.
# TODO nlines should be the number of non-redundant lines so something similar to dim
function isredundant(p::HRep{T}, v::Union{AbstractVector, Line, Ray}; strongly = false, nl::Int=nlines(p), solver=nothing) where {T}
    # v is in every hyperplane otherwise it would not be valid
    hcount = nhyperplanes(p) + count(h -> v in hyperplane(h), halfspaces(p))
    strong = (isray(v) ? fulldim(p) - 1 : fulldim(p)) - nl
    bound = strongly ? min(strong, 1) : strong
    if hcount < bound
        return true
    else
        A = Matrix{T}(undef, hcount, fulldim(p))
        offset = coord_matrix!(A, hyperplanes(p), _ -> true, 0)
        offset = coord_matrix!(A, halfspaces(p), h -> v in hyperplane(h), offset)
        @assert offset == size(A, 1)
        return rank(A) < bound
    end
end
# A line is never redundant but it can be a duplicate
isredundant(p::HRep{T}, v::Line; strongly = false, nl::Int=nlines(p), solver=nothing) where {T} = false

# H-redundancy
# If p is a V-representation, nl needs to be given otherwise if p is a Polyhedron, it can be asked to p.
function isredundant(p::VRep{T}, h::HRepElement; strongly = false, d::Int=dim(p), solver=nothing) where {T}
    checkvconsistency(p)
    hp = hyperplane(h)
    pcount = count(p -> p in hp, points(p))
    # every line is in h, otherwise it would not be valid
    rcount = nlines(p) + count(r -> r in hp, rays(p))
    if pcount < min(d, 1) || (!strongly && pcount + rcount < d)
        return true
    else
        strongly && return false
        A = Matrix{T}(undef, max(0, pcount - 1) + rcount, fulldim(p))
        offset = 0
        if pcount > 1
            orig = nothing
            for x in points(p)
                if x in hp
                    if orig === nothing
                        orig = x
                    else
                        offset += 1
                        @. A[offset, :] = x - orig
                    end
                end
            end
        end
        if !iszero(rcount)
            offset = coord_matrix!(A, rays(p), r -> r in hp, offset)
        end
        @assert offset == size(A, 1)
        return rank(A) + min(1, pcount) < d
    end
end
# An hyperplane is never redundant but it can be a duplicate
isredundant(p::VRep{T}, h::HyperPlane; strongly = false, d::Int=dim(p), solver=nothing) where {T} = false

######################
# Duplicates removal #
######################

export removeduplicates

"""
    removeduplicates(rep::Representation)

Removes the duplicates in the Representation.

* In an H-representation, it removes the redundant hyperplanes and it remove an halfspace when it is equal to another halfspace in the affine hull.
  For instance, `HalfSpace([1, 1], 1)` is equal to `HalfSpace([1, 0], 0)` in the affine hull generated by `HyperPlane([0, 1], 1])`.
* In a V-representation, it removes the redundant lines and it remove a point (resp. ray) when it is equal to another point (resp. ray) in the line hull.
  For instance, in the line hull generated by `Line([0, 1])`, `[1, 1]` is equal to `[1, 0]` and `Ray([2, 2])` is equal to `Ray([1, 0])`.
"""
function removeduplicates end

# V-duplicates
# Function barrier approach:
# Separate function so that it is compiled with a concrete type for p
function vpupdatedup!(aff, points, p::AbstractVector)
    if !any(point -> (point - p) in aff, points)
        push!(points, p)
    end
end
function premovedups(vrep::VRepresentation, aff::VLinearSpace)
    ps = pointtype(vrep)[]
    for p in points(vrep)
        vpupdatedup!(aff, ps, p)
    end
    tuple(ps)
end

function removeduplicates(vrep::VPolytope)
    typeof(vrep)(FullDim(vrep), premovedups(vrep, emptyspace(vrep))...)
end
removeduplicates(vrep::VLinearSpace, solver = nothing) = detectvlinearity(vrep, solver)
function removeduplicates(vrep::VRepresentation, solver = nothing)
    aff, rs = _detect_linearity(vrep, solver)
    typeof(vrep)(FullDim(vrep), premovedups(vrep, aff)..., aff.lines, rs)
end

# H-duplicates
removeduplicates(hr::HRepresentation, solver = nothing) = detecthlinearity(hr, solver)

max_stars_repo_name: blegat/Polyhedra

max_issues_repo_name: blegat/Polyhedra

max_forks_repo_name: blegat/Polyhedra


text:
function sp_delta_pmf(umbrella_center, data_k, kbt, spring_constant)
    nframe = size(data_k[1], 1)
    K = length(data_k)
    delta_pmf      = similar(umbrella_center, nframe, K)
    bias_potential = similar(umbrella_center, nframe, K)
    delta_pmf .= zero(umbrella_center[1])
    bias_potential .= zero(umbrella_center[1])

    for k = 1:K
        dd_mean = mean(data_k[k], dims=1)
        dd_cov = cov(data_k[k])
        #dd_inv = dd_cov \ CuArray{Float64}(I, size(dd_cov))
        dd_inv = dd_cov \ copyto!(similar(dd_cov, size(dd_cov,1), size(dd_cov,1)), I)
        bias_potential = spring_constant * sum((data_k[k] .- umbrella_center[k:k, :]).^2, dims=2)
        d = data_k[k] .- dd_mean
        tmp_pmf = kbt .* (-0.5 .* sum((d * dd_inv) .* d, dims=2))
        delta_pmf[:, k:k] .= .- tmp_pmf .+ tmp_pmf[1, 1] .- bias_potential .+ bias_potential[1, 1]
    end

    return delta_pmf
end

#######################################
function sp_design_matrix(umbrella_center, sim_all_datas, sigma_g; stride=1)
    nframe = size(sim_all_datas[1], 1)
    subframes = range(1, stop=nframe, step=stride)
    #design_matrix = CUDA.zeros(Float64, length(subframes) * size(umbrella_center, 1), size(umbrella_center, 1))
    design_matrix = similar(umbrella_center, length(subframes) * size(umbrella_center, 1), size(umbrella_center, 1))
    design_matrix .= zero(umbrella_center[1])

    for i = 1:size(umbrella_center, 1)
        for k = 1:size(umbrella_center, 1)
            diff = exp.(- 0.5 .* sum((sim_all_datas[i] .- umbrella_center[k:k, :]).^2, dims=2) ./ sigma_g.^2)
            diff .-= exp(- 0.5 .* sum((sim_all_datas[i][1, :] .- umbrella_center[k, :]).^2) ./ sigma_g.^2)
            design_matrix[(length(subframes)*(i-1)+1):(i*length(subframes)), k] = diff
        end
    end

    return design_matrix
end

#######################################
function sp_design_matrix_atom(umbrella_center, sim_all_datas, sigma_g)
    nframe = size(sim_all_datas[1], 1)
    numbrella = size(umbrella_center, 1)
    natom = Int(size(umbrella_center, 2) / 3)
    #design_matrix = CUDA.zeros(Float64, nframe * numbrella, numbrella * natom)
    design_matrix = similar(umbrella_center, nframe * numbrella, numbrella * natom)
    design_matrix .= zero(umbrella_center[1])

    for i = 1:size(umbrella_center, 1)
        for k = 1:size(umbrella_center, 1)
            diff_x1 = exp.(- 0.5 .* ((sim_all_datas[i][:, 1:3:end]   .- umbrella_center[k:k, 1:3:end]).^2) ./ sigma_g.^2)
            diff_y1 = exp.(- 0.5 .* ((sim_all_datas[i][:, 2:3:end]   .- umbrella_center[k:k, 2:3:end]).^2) ./ sigma_g.^2)
            diff_z1 = exp.(- 0.5 .* ((sim_all_datas[i][:, 3:3:end]   .- umbrella_center[k:k, 3:3:end]).^2) ./ sigma_g.^2)
            diff_x2 = exp.(- 0.5 .* ((sim_all_datas[i][1:1, 1:3:end] .- umbrella_center[k:k, 1:3:end]).^2) ./ sigma_g.^2)
            diff_y2 = exp.(- 0.5 .* ((sim_all_datas[i][1:1, 2:3:end] .- umbrella_center[k:k, 2:3:end]).^2) ./ sigma_g.^2)
            diff_z2 = exp.(- 0.5 .* ((sim_all_datas[i][1:1, 3:3:end] .- umbrella_center[k:k, 3:3:end]).^2) ./ sigma_g.^2)
            design_matrix[(nframe*(i-1)+1):(nframe*i), (natom*(k-1)+1):(natom*k)] .= (diff_x1 .* diff_y1 .* diff_z1) .- (diff_x2 .* diff_y2 .* diff_z2)
        end
    end
    return design_matrix
end

#######################################
function funcS(x, lambda)
    return sign(x) * max(abs(x) - lambda, 0.0)
end

#######################################
"""
Least squares
"""
function sp_lsquares(y, X)
    # y = X * weight
    U, S, V = svd(X) # svd
    inverse_M = V * inv(Diagonal(S)) * U' # pseudo-inverse
    beta = inverse_M * y

    residual = sum((y - X*beta).^2)
    return beta, residual
end

"""
Alternating Direction Method of Multipliers (ADMM) for solving lasso
"""
function sp_admm(y, X, lambda=0.1; rho=1.0, condition=1e-5, iter_max=10000)
    ncolumn = size(X, 2);
    #beta = CUDA.zeros(Float64, ncolumn)
    #gamma = CUDA.zeros(Float64, ncolumn)
    #my = CUDA.zeros(Float64, ncolumn)
    beta = similar(y, ncolumn)
    gamma = similar(y, ncolumn)
    my = similar(y, ncolumn)
    beta .= zero(y[1])
    gamma .= zero(y[1])
    my .= zero(y[1])

    old_beta = copy(beta)
    old_gamma = copy(gamma)
    old_my = copy(my)

    B = X' * X
    #B += (I * (ncolumn * rho))
    B += copyto!(similar(y, size(X, 2), size(X, 2)), I) .* (ncolumn .* rho)
    #U, S, V = svd(X' * X + ((CuArray{Float64}(I, (ncolumn, ncolumn)) .* (ncolumn .* rho))))
    U, S, V = svd(B)
    #U, S, V = svd(B, CUDA.CUSOLVER.JacobiAlgorithm)
    #U, S, V = svd(B, CUDA.CUSOLVER.QRAlgorithm)
    pinverse_M = V * inv(Diagonal(S)) * U'
    const_num = pinverse_M
    #const_num = B \ CuArray{Float64}(I, size(B))

    max_diff = 100
    cnt::Int = 1
    X_y = X' * y
    while max_diff > condition
        beta .= const_num * (X_y .+ ((gamma .- (my .* (1.0 ./ rho))) .* (ncolumn .* rho)))
        gamma .= funcS.(beta .+ (my .* (1.0 ./ rho)), lambda ./ rho)
        my .= my .+ ((beta .- gamma) .* rho)

        max_diff = maximum(abs.(gamma .- old_gamma))
        old_beta .= beta
        old_gamma .= gamma
        old_my .= my
        cnt += 1
        if cnt > iter_max
            break
        end
    end

    println("[ Cycle Count = ", cnt, " ]")
    println("[ Complete Condition ]")
    println("  Max Differ = ", max_diff)
    println("\n")

    residual = sum((y - X*beta).^2)

    return gamma, residual
end

#"""
#Coordinate descent
#"""
#function sp_descent(y, X, lambda=0.1; condition=1e-5, iter_max=10000)
#    nframe = size(X, 1)
#    nfeature = size(X, 2)
#    rhs1 = X' * y
#    B = X' * X
#
#    beta = similar(y, nfeature)
#    beta_old = similar(y, nfeature)
#    beta_old .= one(y[1])
#    rhs2 = similar(y, nfeature)
#    rhs3 = similar(y, nfeature)
#    rhs = similar(y, nfeature)
#    max_diff = 100.0
#    cnt = 1
#    while max_diff > condition
#        rhs2 .= B * beta_old
#        rhs3 .= nframe .* beta_old
#        rhs .= rhs1 .- rhs2 .+ rhs3
#        #@show rhs./nframe
#        beta .= funcS.(rhs./nframe, lambda)
#        #@show beta
#        max_diff = maximum(abs.(beta .- beta_old))
#        beta_old .= beta
#        cnt += 1
#        if cnt > iter_max
#            break
#        end
#    end
#
#    println("[ Cycle Count = ", cnt, " ]")
#    println("[ Complete Condition ]")
#    println("  Max Differ = ", max_diff)
#    println("\n")
#
#    return beta
#end

"""
Coordinate descent
"""
function sp_descent(y, X, lambda=0.1; condition=1e-5, iter_max=10000)
    nframe = size(X, 1)
    nfeature = size(X, 2)
    Xty = X' * y
    XtX = X' * X

    beta = similar(y, nfeature)
    beta_old = similar(y, nfeature)
    beta .= one(y[1])
    beta_old .= beta
    #rhs2 = similar(y, nfeature)
    #rhs3 = similar(y, nfeature)
    rhs = similar(y, nfeature)
    max_diff = 100.0
    cnt = 1
    while max_diff > condition
        cnt += 1
        #@show cnt, max_diff
        for j = 1:nfeature
            rhs1 = Xty[j]
            rhs2 =  sum(XtX[j, :] .* beta)
            rhs3 = nframe * beta[j]
            rhs = rhs1 - rhs2 + rhs3
            beta[j] = funcS(rhs/nframe, lambda)
        end
        max_diff = maximum(abs.(beta .- beta_old))
        beta_old .= beta
        if cnt > iter_max
            break
        end
    end

    println("[ Cycle Count = ", cnt, " ]")
    println("[ Complete Condition ]")
    println("  Max Differ = ", max_diff)
    println("\n")

    residual = sum((y - X*beta).^2)

    return beta, residual
end

#######################################
function sp_standardize!(M)
    mean_M = mean(M, dims=1)
    std_M = sqrt.(sum((M .- mean_M).^2, dims=1)) ./ sqrt(size(M, 1))
    M .= (M .- mean_M) ./ std_M
    return mean_M, std_M
end

#######################################
function sp_standardize(M)
    mean_M = mean(M, dims=1)
    #std_M = std(M, dims=1)
    std_M = sqrt.(sum((M .- mean_M).^2, dims=1)) ./ sqrt(size(M, 1))
    M_standardized = (M .- mean_M) ./ std_M
    return M_standardized, mean_M, std_M
end

#######################################
function sp_cumulate_pmf(x, weight, umbrella_center, sigma_rdf, mean_M, std_M)
    ndim = size(umbrella_center, 2)
    K = size(umbrella_center, 1)
    nframe = size(x, 1)

    pmf = zeros(Float64, nframe);
    for iframe = 1:nframe
        sum_rdf = 0.0
        for k = 1:K
            tmp = (exp(sum(- 0.5 .* (x[iframe, :] .- umbrella_center[k, :]).^2 ./ sigma_rdf.^2))  - mean_M[k]) / std_M[k]
            sum_rdf += tmp * weight[k]
        end
        pmf[iframe] = sum_rdf
    end

    return pmf .- minimum(pmf)
end

#######################################
function sp_cumulate_pmf_atom(data, weight, umbrella_center, sigma_rdf, mean_M, std_M)
    ndim = size(umbrella_center, 2)
    natom = Int(ndim / 3)
    natom3 = natom * 3
    K = size(umbrella_center, 1)
    nframe = size(data, 1)

    pmf = zeros(Float64, nframe);
    for iframe = 1:nframe
        sum_rdf = 0.0
        for k = 1:K
            index = ((k-1)*natom+1):(k*natom)
            index_x = 1:3:natom3
            index_y = 2:3:natom3
            index_z = 3:3:natom3
            tmp   = (data[iframe:iframe, index_x] .- umbrella_center[k:k, index_x]).^2
            tmp .+= (data[iframe:iframe, index_y] .- umbrella_center[k:k, index_y]).^2
            tmp .+= (data[iframe:iframe, index_z] .- umbrella_center[k:k, index_z]).^2
            tmp ./= sigma_rdf.^2
            tmp .*= -0.5
            tmp .= (exp.(tmp) .- mean_M[1:1, index]) ./ std_M[1:1, index]
            sum_rdf += sum(tmp[:] .* weight[index])
            #for iatom = 1:natom
            #    index = (k-1)*natom + iatom
            #    index3 = ((iatom-1)*3 + 1):(iatom*3)
            #    tmp = (exp(sum(- 0.5 .* (data[iframe, index3] .- umbrella_center[k, index3]).^2 ./ sigma_rdf.^2))  - mean_M[index]) / std_M[index]
            #    sum_rdf += tmp * weight[index]
            #end
        end
        pmf[iframe] = sum_rdf
    end

    return pmf .- minimum(pmf)
end

#######################################
function sp_lasso(M, delta_pmf, sigma_rdf)
end


text:
using Xpress, JuMP, MathProgBase, Compat.Test, OffsetArrays
import Xpress.XpressSolver

lp_solvers = Any[]
ip_solvers = Any[]
ip_dual_solvers = Any[]
semi_solvers = Any[]
sos_solvers = Any[]
conic_solvers_with_duals = Any[]
lazy_solvers, lazylocal_solvers, cut_solvers, cutlocal_solvers, heur_solvers, info_solvers = Any[], Any[], Any[], Any[], Any[], Any[]
quad_solvers = Any[]
quad_mip_solvers = copy(quad_solvers)
soc_solvers = copy(quad_solvers)
quad_soc_solvers = copy(quad_solvers)
rsoc_solvers = Any[]
nlp_solvers = Any[]
convex_nlp_solvers = copy(nlp_solvers)
minlp_solvers = Any[]
sdp_solvers = Any[]
error_map = Dict()
grb, cpx, cbc, glp, mos = false, false, false, false, false
xpr = true

lp_solvers = [Xpress.XpressSolver()]
ip_solvers = [Xpress.XpressSolver()]

sos_solvers = [Xpress.XpressSolver()]

semi_solvers = [Xpress.XpressSolver()]

soc_solvers = [Xpress.XpressSolver()]
rsoc_solvers = [Xpress.XpressSolver()]

tol = 1e-12

quad_solvers = [Xpress.XpressSolver(FEASTOL = tol, BARPRIMALSTOP = tol, BARGAPSTOP = tol, BARDUALSTOP = tol)]
quad_soc_solvers = [Xpress.XpressSolver(FEASTOL = tol, BARPRIMALSTOP = tol, BARGAPSTOP = tol, BARDUALSTOP = tol)]
quad_mip_solvers = [Xpress.XpressSolver(FEASTOL = tol, BARPRIMALSTOP = tol, BARGAPSTOP = tol, BARDUALSTOP = tol)]

include(Pkg.dir("JuMP","test","model.jl"))
include(Pkg.dir("JuMP","test","probmod.jl"))

include(Pkg.dir("JuMP","test","socduals.jl"))
include(Pkg.dir("JuMP","test","qcqpmodel.jl"))


text:
using OrdinaryDiffEq

function lorenz!(du,u,p,t)
 du[1] = 10.0*(u[2]-u[1])
 du[2] = u[1]*(28.0-u[3]) - u[2]
 du[3] = u[1]*u[2] - (8/3)*u[3]
end

u0 = BigFloat[1.0;0.0;0.0]
tspan = (big(0.0),big(100.0))
prob = ODEProblem(lorenz!,u0,tspan)
sol = solve(prob,Tsit5(),save_everystep=false)

x = sol.u[end]

import LinearAlgebra.norm
function condition(u,t,integrator)
    norm(u-x)-0.1
end

affect!(integrator) = terminate!(integrator)

cb = ContinuousCallback(condition,affect!)

sol2 = solve(prob,Tsit5(),save_everystep=false,callback=cb)


text:
using StatsBase , JLD,  Dates, PyPlot, DelimitedFiles
        using  Utilities

        # script that loads the csv file with emid data and cleans it:
        #
        # --- aggregate the ON loans in weekly adjacency matrices
        # --- selecti only banks that are active at least a given fraction of the weeks
        # --- Store also a half period version of the data, starting after LTRO began

        halfPeriod = false # Consider only data after LTRO began ?
        ####--- Load Data from txt files
        ##load Dates
        loadPath = "/home/Domenico/Dropbox/Dynamic_Networks/data/emid_data/csvFiles/banksNames.txt"
        banksIDs = Array{Int}(readdlm(loadPath,','))
        NallBanks = length(unique(banksIDs))
        N = NallBanks
        loadPath = "/home/Domenico/Dropbox/Dynamic_Networks/data/emid_data/csvFiles/dates.txt"
        tmp = Array{Int}(readdlm(loadPath,','))
        alldatesONeMid = Array{Date,1}(undef,length(tmp) ) # vector of the dates in the data
        for i=1:length(tmp) alldatesONeMid[i] = Date("$(tmp[i])","yyyymmdd") end # convert it in dates type
        TdaysAll = length(unique(alldatesONeMid))
        halfPeriod ? startDate = Date("2012-03-12") : startDate=alldatesONeMid[1] #
        endDate = Date("2015-02-27")
        Dates.dayname(startDate) # The first day in the data is a
        timesToKeep = (alldatesONeMid .>=startDate) .&(alldatesONeMid .<=endDate)
        Tdays = sum(timesToKeep)
        datesONeMid = alldatesONeMid[timesToKeep]
        ## Load Matrices
        loadPath = "/home/Domenico/Dropbox/Dynamic_Networks/data/emid_data/csvFiles/eMid_all_T.txt"

tmp = readdlm(loadPath,',')
YeMidDaylyAll_T = reshape(tmp,NallBanks,NallBanks,TdaysAll)
YeMidDaylyAll_T = YeMidDaylyAll_T[:,:,timesToKeep]
## Aggregate Matrices weekly
fracWeek =  [tmp.value for tmp in (datesONeMid .- datesONeMid[1])]/7# to wich week each day belongs to ?
[Dates.dayname.(datesONeMid) fracWeek floor.(fracWeek)]
weekInd = 1 .+ floor.(Int,fracWeek)
weekInd[end] == length(unique(weekInd)) ? () : error()
Tweeks = weekInd[end]
YeMidWeeklyAll_T = zeros(NallBanks,NallBanks,Tweeks)
for t = 1:Tweeks YeMidWeeklyAll_T[:,:,t] = sum(YeMidDaylyAll_T[:,:,weekInd .== t],dims=3) end
date_first_day_week = [datesONeMid[weekInd .== t][1] for t =1:Tweeks  ]# t = 1:Tweeks  = sum(YeMidDaylyAll_T[:,:,weekInd .== t],dims=3) end
days = day.(date_first_day_week)
months = month.(date_first_day_week)
years = year.(date_first_day_week)

#Some banks' statistics:
AeMidWeeklyAll_T = YeMidWeeklyAll_T[:,:,:] .> 0
degsI_T = sumSq(AeMidWeeklyAll_T,2)
degsO_T = sumSq(AeMidWeeklyAll_T,1)
strI_T = sumSq(YeMidWeeklyAll_T,2)
strO_T = sumSq(YeMidWeeklyAll_T,1)
# Select only banks active for more than a fixed proportion of the times
activityThreshold = 0.05# 0.9999
isBankiActiveAtTimet =  (degsI_T.>0) .| (degsO_T.>0)
activityBanks = sumSq(isBankiActiveAtTimet,2)./(length(isBankiActiveAtTimet[1,:]))
inactiveBanks = activityBanks .<= activityThreshold; NinactiveBanks = sum(inactiveBanks)
N = NallBanks - NinactiveBanks

##

#compute the strengths and degrees for the sub network of active banks!!

YeMidWeekly_T = YeMidWeeklyAll_T[.!inactiveBanks,.!inactiveBanks,:]
AeMidWeekly_T = YeMidWeekly_T[:,:,:] .> 0
degsI_T = sumSq(AeMidWeekly_T,2)
degsO_T = sumSq(AeMidWeekly_T,1)
strI_T = sumSq(YeMidWeekly_T,2)
strO_T = sumSq(YeMidWeekly_T,1)
degsIO_T = [degsI_T; degsO_T]
strIO_T = [strI_T; strO_T]
~,meandegsI,meandegsO = splitVec(meanSq(degsIO_T,2))

sum(strI_T)-sum(strO_T) >1e-12 ? error() : ()
save_fold =   "/home/Domenico/Dropbox/Dynamic_Networks/data/emid_data/juliaFiles/"
file_nameStart = "Weekly_eMid_Data"
file_nameEnd = "_from_$(Dates.format(startDate,"yyyy_mm_dd"))"*
        "_to_" *"$(Dates.format(endDate,"yyyy_mm_dd"))"*".jld"

# save Data
save_path = save_fold*file_nameStart  *file_nameEnd#
@save(save_path, AeMidWeekly_T,banksIDs,inactiveBanks, YeMidWeekly_T,weekInd,
datesONeMid ,degsIO_T,strIO_T, years, months, days)

max_stars_repo_name: domenicodigangi/SDERGM

max_issues_repo_name: domenicodigangi/SDERGM

max_forks_repo_name: domenicodigangi/SDERGM


text:
function stepsize_controller!(integrator::SDEIntegrator, controller::PIController, alg)
    integrator.q11 = DiffEqBase.value(DiffEqBase.fastpow(integrator.EEst,controller.beta1))
    integrator.q = DiffEqBase.value(integrator.q11/DiffEqBase.fastpow(integrator.qold,controller.beta2))
    @fastmath integrator.q = DiffEqBase.value(max(inv(integrator.opts.qmax),min(inv(integrator.opts.qmin),integrator.q/integrator.opts.gamma)))
end

@inline function step_accept_controller!(integrator::SDEIntegrator, alg)
    step_accept_controller!(integrator, integrator.opts.controller, alg)
end

function step_accept_controller!(integrator::SDEIntegrator, controller::PIController, alg)
    integrator.dtnew = DiffEqBase.value(integrator.dt/integrator.q) * oneunit(integrator.dt)
end

function step_reject_controller!(integrator::SDEIntegrator, controller::PIController, alg)
    integrator.dtnew = integrator.dt/min(inv(integrator.opts.qmin),integrator.q11/integrator.opts.gamma)
end

function stepsize_controller!(integrator::SDEIntegrator, alg::TauLeaping)
    nothing
end

function step_accept_controller!(integrator::SDEIntegrator, alg::TauLeaping)
    integrator.q = min(integrator.opts.gamma / integrator.EEst, integrator.opts.qmax)
    return integrator.dt * integrator.q
end

function step_reject_controller!(integrator::SDEIntegrator, alg::TauLeaping)
    integrator.dt = integrator.opts.gamma * integrator.dt / integrator.EEst
end

function stepsize_controller!(integrator::SDEIntegrator, alg::CaoTauLeaping)
    nothing
end

function step_accept_controller!(integrator::SDEIntegrator, alg::CaoTauLeaping)
    return integrator.EEst # use EEst for the τ
end

function step_reject_controller!(integrator::SDEIntegrator, alg::CaoTauLeaping)
    error("CaoTauLeaping should never reject steps")
end

max_stars_repo_name: JuliaDiffEq/StochasticDiffEq