"""
    add_constraints!(ata_model::AbstractModel; constraints_file = "constraints.csv", constraints_delim = ";")

# Description

Add categorical and sum constraints to the `ATA.AbstractModel` as specified in the `constraints_file`.
Alternatively, the constraint dataframe can be passed using the argument `constraints`.
It requires the [`start_ata`](#ATA.start_ata) step.

# Arguments

- **`ata_model::AbstractModel`** : Required. The model built with `start_ata()` and with settings loaded by [`start_ata`](#ATA.start_ata) function.
- **`constraints::DataFrames.DataFrame = DataFrames.DataFrame()**`. Optional. Default: DataFrames.DataFrame(). The dataframe containing the categorical and quantitative constraints.
- **`constraints_file`** : Optional. Default: "constraints.csv". The path of the file containing the categorical and sum constraints in the form of custom-separated values.
- **`constraints_delim`** : Optional. Default: ";". The custom-separator for the `constraints_file`.

"""
function add_constraints!(
    ata_model::AbstractModel;
    constraints::DataFrames.DataFrame = DataFrames.DataFrame(),
    constraints_file = "constraints.csv",
    constraints_delim = ";",
)
    if size(constraints, 1) > 0
        categorical_constraints = constraints
        success!(ata_model, "Constraints dataframe loaded.")
    elseif !isfile(constraints_file)
        error!(
            ata_model,
            string(
                "Constraints file with name ",
                constraints_file,
                " does not exist. Provide a valid constraints dataframe or a name of an existing file.",
            ),
        )
        return nothing
    else
        try
            categorical_constraints =
                CSV.read(constraints_file, DataFrames.DataFrame, delim = constraints_delim)
        catch e
            error!(ata_model, "Error in reading constraints file.")
            return nothing
        end
        success!(ata_model, "Constraints file loaded.")
    end
    try
        n_items = ata_model.settings.n_items
        bank = ata_model.settings.bank
        A = Vector{Matrix{Float64}}(undef, ata_model.settings.T)
        b = Vector{Vector{Float64}}(undef, ata_model.settings.T)
        if size(ata_model.constraints[1].constr_b, 1) > 0
            for t = 1:ata_model.settings.T
                A[t] = copy(ata_model.constraints[t].constr_A)
                b[t] = copy(ata_model.constraints[t].constr_b)
            end
        else
            for t = 1:ata_model.settings.T
                A[t] = zeros(Float64, 0, ata_model.settings.n_items)
                b[t] = Vector{Float64}(undef, 0)
            end
        end
        x_forced0 = ata_model.settings.forced0
        if size(categorical_constraints, 1) == 0
            error!(
                ata_model,
                string("No lines in file ", constraints_file, " nothing added."),
            )
        else
            categorical_constraints.var = Symbol.(categorical_constraints.var)
            CatCons = Vector{Vector{Float64}}(undef, ata_model.settings.T)
            for g = 1:ata_model.settings.n_groups
                if g > 1
                    tests = collect(
                        (sum(
                            ata_model.settings.Tg[1:(g-1)],
                        )+1):(sum(
                            ata_model.settings.Tg[1:(g-1)],
                        )+ata_model.settings.Tg[g]),
                    )
                else
                    tests = collect(1:ata_model.settings.Tg[1])
                end
                GroupCatCons = findall((categorical_constraints.group .== g))
                for con in GroupCatCons
                    for t in tests
                        if !ismissing(categorical_constraints[con, :min])
                            indices =
                                string.(
                                    bank[!, Symbol.(categorical_constraints[con, :var])],
                                ) .== string(categorical_constraints[con, :value])
                            indices[ismissing.(indices)] .= false
                            indices = findall(indices .== true)
                            A[t] = vcat(A[t], zeros(Float64, 1, n_items))
                            if ismissing(categorical_constraints[con, :weight])
                                A[t][end, indices] .= -1.0
                                push!(b[t], -categorical_constraints[con, :min])
                            else
                                A[t][end, indices] .=
                                    -(categorical_constraints[con, :weight])
                                push!(
                                    b[t],
                                    -categorical_constraints[con, :weight] *
                                    categorical_constraints[con, :min],
                                )
                            end
                        end
                        if !ismissing(categorical_constraints[con, :max])
                            indices =
                                string.(
                                    ata_model.settings.bank[
                                        !,
                                        Symbol.(categorical_constraints[con, :var]),
                                    ],
                                ) .== string(categorical_constraints[con, :value])
                            indices[ismissing.(indices)] .= false
                            indices = findall(indices .== true)
                            if categorical_constraints[con, :max] == 0
                                x_forced0[t][indices] .= false
                            else
                                A[t] = vcat(A[t], zeros(Float64, 1, n_items))
                                if ismissing(categorical_constraints[con, :weight])
                                    A[t][end, indices] .= 1.0
                                    push!(b[t], categorical_constraints[con, :max])
                                else
                                    A[t][end, indices] .=
                                        (categorical_constraints[con, :weight])
                                    push!(
                                        b[t],
                                        categorical_constraints[con, :weight] *
                                        categorical_constraints[con, :max],
                                    )
                                end
                            end
                        end
                    end
                end
            end
            success!(ata_model, "Linear constraints (categorical and quantitative) added.")
        end

        for t = 1:ata_model.settings.T
            DelimitedFiles.writedlm("opt/A_$t.csv", A[t])
            DelimitedFiles.writedlm("opt/b_$t.csv", b[t])
        end
        DelimitedFiles.writedlm("opt/x_forced0.txt", x_forced0)
        #update model
        ata_model.settings.forced0 = x_forced0
        for t = 1:ata_model.settings.T
            ata_model.constraints[t].constr_b = b[t]
            ata_model.constraints[t].constr_A = A[t]
        end
        JLD2.@save "opt/ata_model.jld2" ata_model

    catch e
        error!(ata_model, string("", sprint(showerror, e)))
    end
    return nothing
end


module BHAtp

using Reexport

@reexport using PGFPlotsX, LaTeXStrings
@reexport using DataFrames, DataFramesMeta
@reexport using Interpolations
@reexport using BHAtp, CSV, Distributed

# Types and derived types

include("Types.jl")
include("Parameters.jl")
include("Dataframes.jl")

# Exported methods

include("Bha.jl")
include("TheoreticalPerformance.jl")
include("ShowFunctions.jl")

# Base level methods

include("InertiaForces.jl")
include("CurvatureForces.jl")
include("InitialRelease.jl")
include("ExceedanceAdjustment.jl")
include("FinalRelease.jl")
include("Fem.jl")
include("TpRunSetup.jl")
include("Finalinit.jl")
include("CreateMaterialTable.jl")
include("CreateMediaTable.jl")
include("CheckInput.jl")
include("CreateMesh.jl")
include("Interpolate.jl")
include("Xyinit.jl")
include("DalphaInit.jl")
include("Stiffness.jl")
include("GeometricMatrix.jl")
include("Weightforces.jl")
include("Endforces.jl")
include("Savefig.jl")

export
  BHAJ,
  tp!,
  show_solution,
  show_tp,
  create_final_tp_df,
  create_element_df,
  create_node_df,
  savefig

end # module


module CTImages

export rescale!, rescale, rotate
export polar2cart
export AbstractCTImage, CTImage, CTSinogram, CTTomogram
export CTImageOrTomog, CTImageMat, CTSinogMat, CTTomogMat
export ctimage, ctsinogram, cttomogram

using IntervalSets, SimpleTraits
import ..Monads: mjoin
using ..Utils: linspace, half, _compute_radius, _compute_angle, _wrap_angle
import ..Utils: _atype
using ..Applicative, ..Monads, ..Interpolation
using ..Geometry:
    AbstractParallelBeamGeometry, AbstractFanBeamGeometry, ParallelBeamGeometry
import Base: size, convert, similar, eltype, getindex, setindex!, IndexStyle
using Base: @propagate_inbounds

function ctimage end
function ctsinogram end
function cttomogram end

abstract type AbstractCTImage{T<:Number} <: AbstractMatrix{T} end

@traitimpl Monad{AbstractCTImage}
mjoin(img::AbstractCTImage) = img.data

# struct CTImageStyle <: Broadcast.AbstractArrayStyle{2} end
# Base.BroadcastStyle(::Type{<:AbstractCTImage}) = CTImageStyle()
# CTImageStyle(::Val{2}) = CTImageStyle()
# CTImageStyle(::Val{N}) where N = Broadcast.DefaultArrayStyle{N}()

_atype(img::M) where {M <: AbstractCTImage} = _atype(M)
eltype(::Type{M}) where {M<:AbstractCTImage{T}} where T = T
size(img::AbstractCTImage) = mbind(size, img)
size(img::AbstractCTImage, dim::Int) = size(mjoin(img), dim)
@propagate_inbounds function getindex(img::AbstractCTImage, i::Int)
    @boundscheck checkbounds(img, i)
    @inbounds getindex(mjoin(img), i)
end
@propagate_inbounds function getindex(
    img::AbstractCTImage, I::Vararg{Int,2}
)
    @boundscheck checkbounds(img, I...)
    @inbounds getindex(mjoin(img), I...)
end
@propagate_inbounds function setindex!(img::AbstractCTImage, v, i::Int)
    @boundscheck checkbounds(img, i)
    @inbounds setindex!(mjoin(img), v, i)
end
@propagate_inbounds function setindex!(
    img::AbstractCTImage, v, I::Vararg{Int,2}
)
    @boundscheck checkbounds(img, I...)
    @inbounds setindex!(mjoin(img), v, I...)
end

const ctnames = (image = :CTImage, sinog = :CTSinogram, tomog = :CTTomogram)
const ctfn = (image = :ctimage, sinog = :ctsinogram, tomog = :cttomogram)

for nm in ctnames
    @eval begin
        struct $nm{T<:Number,M<:AbstractMatrix{T}} <: AbstractCTImage{T}
            data::M
        end
        @inline _atype(::Type{$nm{T,M}}) where {T,M} = M
        IndexStyle(::Type{T}) where T<:$nm = IndexStyle(_atype(T))
        @inline function $nm{T,M}(
            ::UndefInitializer,
            dims::Vararg{Union{Integer,AbstractUnitRange},2}
        ) where {T,M}
            $nm(M(undef, dims...))
        end
        @inline $nm{T,M}(img::$nm) where {T,M} = $nm{T,M}(mjoin(img))
        @inline $nm{T}(img::M) where {T,M<:AbstractMatrix{T}} = $nm{T,M}(img)
        @inline $nm(img::$nm) = img
        @inline convert(::Type{M}, img::AbstractMatrix) where {M<:$nm} =
            mreturn(M, img)
        @inline convert(::Type{M}, img::$nm) where {M<:$nm} =
            mreturn($nm, convert(_atype(M), mjoin(img)))
        @inline function similar(img::M) where {M<:$nm}
            mreturn(M, similar(mjoin(img)))
        end
        @inline function similar(
            img::M,
            ::Type{S},
            dims::Tuple{Vararg{Int,2}}
        ) where {S,M<:$nm}
            mreturn(M, similar(mjoin(img), S, dims))
        end
    end
end

CTImage(::Union{CTSinogram,CTTomogram}) =
    @error "Cannot construct a CT image from a sinogram or a tomogram"
CTSinogram(::Union{CTImage,CTTomogram}) =
    @error "Cannot construct a CT sinogram from an image or a tomogram"
CTTomogram(::Union{CTImage,CTSinogram}) =
    @error "Cannot construct a CT tomogram from an image or a sinogram"

const CTImageOrTomog{T,M} = Union{CTImage{T,M},CTTomogram{T,M}}
const CTImageMat{T} = CTImage{T,Matrix{T}}
const CTSinogMat{T} = CTSinogram{T,Matrix{T}}
const CTTomogMat{T} = CTTomogram{T,Matrix{T}}

"""
    rotate(mat::AbstractMatrix, α::Real; <keyword arguments>)

Rotate matrix `mat` about the center of angle `α` given in degrees.
If `rows` and `cols` are not given, the rotated matrix has the same
dimensions of the original matrix.

# Arguments
- `mat`: matrix to rotate.
- `α`: angle in degrees.
- `rows=nothing`: number of rows of the rotated matrix.
- `cols=nothing`: number of columns of the rotated matrix.
- `interpolation`: interpolation strategy. By default is
    `BilinearInterpolation`.
"""
function rotate(
    mat::AbstractMatrix{T},
    α::Real;
    rows::Optional{Integer} = nothing,
    cols::Optional{Integer} = nothing,
    background::Optional{Real} = nothing,
    interpolation::Optional{Interp} = nothing,
) where {T <: Number,Interp <: AbstractInterp2DOrNone}
    orows, ocols = size(mat)
    rows = maybe(orows, rows)
    cols = maybe(ocols, cols)
    sϕ, cϕ = sincos(deg2rad(α))
    interpolation = maybe(interpolate, interpolation)
    interp = interpolation(mat)
    x₀::T = (cols + 1) / 2
    y₀::T = (rows + 1) / 2
    x′₀::T = (ocols + 1) / 2
    y′₀::T = (orows + 1) / 2
    z::T = maybe(zero(T), background)
    rmat = similar(mat, rows, cols)
    fill!(rmat, z)
    for ix ∈ 1:rows, iy ∈ 1:cols
        x = T(ix) - x₀
        y = T(iy) - y₀
        x′ = x * cϕ + y * sϕ + x′₀
        y′ = y * cϕ - x * sϕ + y′₀
        if x′ ∈ 1..ocols && y′ ∈ 1..orows
            rmat[iy, ix] = interp(y′, x′)
        end
    end
    rmat
end

"""
    rescale!(x::AbstractArray;
        interval=nothing, calibration=nothing, window=nothing)

Rescale array x to the interval specified by `interval`.

If `calibration` is not `nothing`, then rescaling is done with
reference to the values given by `calibration`. In other words,
minimum and maximum are assumed to be the values specified by
`calibration`.

See also: [`rescale`](@ref)
"""
function rescale!(
    img::AbstractArray{T};
    interval::ClosedInterval = zero(T)..one(T),
    calibration::Optional{ClosedInterval{U}} = nothing,
    window::Optional{ClosedInterval{W}} = nothing,
) where {T <: Number,U <: Number,W <: Number}
    if isnothing(calibration)
        calibration = ClosedInterval(extrema(img)...)
    end
    a, b = T.(endpoints(interval))
    m, M = T.(endpoints(calibration))
    if m == M
        @warn "Cannot calibrate image as calibration values ($calibration) are equal, image unchanged"
        return img
    end
    f = (b - a) / (M - m)
    if m != zero(T)
        img .-= m
    end
    img .*= f
    if a != zero(T)
        img .+= a
    end
    isnothing(window) && return img
    a, b = T.(endpoints(window))
    map!(img, img) do x
        x < a && return a
        x > b && return b
        return x
    end
    img
end

"""
    rescale(x::AbstractArray, slope::Number, intercept::Number; window)

Linear rescaling of `x` as `x * slope + intercept`.

See also: [`rescale!`](@ref)
"""
function rescale(
    image::AbstractArray{T},
    slope::Number,
    intercept::Number;
    window::Optional{ClosedInterval{U}} = nothing,
) where {T <: Number,U <: Number}
    res = @. image * slope + intercept
    isnothing(window) && return res
    a, b = T.(endpoints(window))
    map!(res, res) do x
        x < a && return a
        x > b && return b
        return x
    end
end

"""
    rescale!(x::AbstractArray, slope::Number, intercept::Number)

In place linear rescaling of `x`.

See also: [`rescale`](@ref)
"""
function rescale!(
    img::AbstractArray{T},
    slope::Number,
    intercept::Number;
    window::Optional{ClosedInterval{U}} = nothing,
) where {T <: Number,U <: Number}
    @. img = slope * img + intercept
    isnothing(window) && return img
    a, b = T.(endpoints(window))
    map!(img, img) do x
        x < a && return a
        x > b && return b
        return x
    end
    img
end

"""
    rescale(x::AbstractArray;
        interval=nothing, calibration=nothing, window=nothing)

Rescale array x to the interval specified by `interval`.

If `calibration` is not `nothing`, then rescaling is done with
reference to the values given by `calibration`. In other words,
minimum and maximum are assumed to be the values specified by
`calibration`.

See also: [`rescale!`](@ref)
"""
function rescale(
    x::AbstractArray;
    interval = 0..1,
    calibration = nothing,
    window = nothing,
)
    rescale!(deepcopy(x); interval, calibration, window)
end

function polar2cart(
    mp::M,
    xs::AbstractVector,
    ys::AbstractVector;
    ν::Real = 1,
    scale::Optional{Real} = nothing,
    diag::Bool = false,
    background::Optional{Real} = nothing,
    transposed::Bool = false,
    interpolation::Optional{Interp} = nothing,
)::M where {
    M <: AbstractMatrix{T},
    Interp <: AbstractInterp2DOrNone,
} where {T <: Real}
    if transposed
        nr, nθ = size(mp)
        mp′ = similar(mp, nθ, nr)
        permutedims!(mp′, mp, (2,1))
        return polar2cart(mp′, xs, ys; background, interpolation)
    end
    ν = maybe(ν, scale)
    @assert ν > 0
    nθ, nr = size(mp)
    x₀, y₀ = half(xs), half(ys)
    κ::T = min(x₀, y₀)
    if diag
        ν *= hypot(x₀, y₀) / κ
    end
    δri::T = (nr-1) / (κ * ν)
    δθi::T = nθ / 2π
    xs′ = xs * δri
    ys′ = ys * δri
    nt1 = nθ + 1
    nthalf = nθ + 1//2
    interpolation = maybe(interpolate, interpolation)
    interp = interpolation(mp)
    rows, cols = length(ys), length(xs)
    z::T = maybe(zero(T), background)
    mc = similar(mp, rows, cols)
    fill!(mc, z)
    Threads.@threads for ix ∈ axes(mc, 2)
        @inbounds @fastmath @simd for iy ∈ axes(mc, 1)
            x::T = xs′[ix]
            y::T = ys′[iy]
            r::T = _compute_radius(x, y)
            θ::T = _compute_angle(x, y, δθi)
            if 1 <= r <= nr && 1 <= θ <= nt1
                if θ >= nthalf
                    θ = one(T)
                elseif θ > nθ
                    θ = nθ
                end
                mc[iy,ix] = interp(θ, r)
            end
        end
    end
    mc
end

@inline function polar2cart(
    mp::AbstractMatrix{T};
    rows::Optional{Integer} = nothing,
    cols::Optional{Integer} = nothing,
    transposed::Bool = false,
    kwargs...
) where {T <: Real,Interp <: AbstractInterp2DOrNone}
    mp = transposed ? permutedims(mp) : mp
    rows = isnothing(rows) ? maybe(2 * size(mp, 2), cols) : rows
    cols = maybe(rows, cols)
    y₀, x₀ = sincos(atan(rows, cols))
    xs = linspace(T, -x₀..x₀, cols)
    ys = linspace(T, -y₀..y₀, rows)
    polar2cart(mp, xs, ys; kwargs...)
end

function polar2cart(
    image::AbstractMatrix,
    geometry::AbstractParallelBeamGeometry;
    kwargs...
)
    polar2cart(image; geometry.rows, geometry.cols, kwargs...)
end

function polar2cart(
    image::AbstractMatrix,
    geometry::AbstractFanBeamGeometry;
    kwargs...
)
    polar2cart(image, ParallelBeamGeometry(geometry); kwargs...)
end

polar2cart(; kwargs...) = x -> polar2cart(x; kwargs...)
polar2cart(g::AbstractParallelBeamGeometry; kwargs...) =
    x -> polar2cart(x, g; kwargs...)

end # module


using BinaryBuilder

name = "PROPACK"
version = v"0.1"

sources = [
    ArchiveSource("https://github.com/optimizers/PROPACK/archive/v1.0.tar.gz",
                  "0d029a4c2cdcdb9b18a4fae77593a562f79406c3f79839ee948782b37974a10e")
]

# Bash recipe for building across all platforms
script = raw"""
cd $WORKSPACE/srcdir/PROPACK-*/

OPENBLAS="-lopenblas"
FFLAGS=(-xf77-cpp-input)
if [[ ${nbits} == 64 ]] && [[ ${target} != aarch64* ]]; then
  OPENBLAS="-lopenblas64_"
  FFLAGS+=(-fdefault-integer-8 -fno-align-commons)

  for sym in caxpy cdotc cdotu ccopy cgemv clarnv clascl cscal csscal daxpy dbdsdc dbdsqr dcopy ddot dgemm dgemv dlamch dlapy2 dlarnv dlartg dlascl dnrm2 dznrm2 drot dscal lsame saxpy sbdsdc sbdsqr scnrm2 scopy sdot sgemm sgemv slamch slapy2 slarnv slartg slascl snrm2 srot sscal zaxpy zcopy zdotc zdotu zdscal zgemv zlarnv zlascl zscal
  do
    FFLAGS+=("-D${sym}=${sym}_64")
  done
fi

make SLIB=${dlext} FFLAG=\"${FFLAGS[*]}\" BLAS=${OPENBLAS}
cp complex8/libcpropack.${dlext} complex16/libzpropack.${dlext} single/libspropack.${dlext} double/libdpropack.${dlext} ${libdir}/
"""

# These are the platforms we will build for by default, unless further
# platforms are passed in on the command line
platforms = supported_platforms()

platforms = expand_gfortran_versions(platforms)

products = [
    LibraryProduct("libcpropack", :libcpropack),
    LibraryProduct("libzpropack", :libzpropack),
    LibraryProduct("libspropack", :libspropack),
    LibraryProduct("libdpropack", :libdpropack),
]

dependencies = [
    Dependency("OpenBLAS_jll"),
]

# Build the tarballs, and possibly a `build.jl` as well.
build_tarballs(ARGS, name, version, sources, script, platforms, products, dependencies)


# Displaying color swatches
# -------------------------

const max_width = 180
const max_height = 150
const max_pixel_size = 25

function Base.show(io::IO, ::MIME"image/svg+xml", c::Color)
    write(io,
        """
        <?xml version="1.0" encoding="UTF-8"?>
        <!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN"
         "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
        <svg xmlns="http://www.w3.org/2000/svg" version="1.1"
             width="25mm" height="25mm" viewBox="0 0 1 1">
             <rect width="1" height="1"
                   fill="#$(hex(c))" stroke="none"/>
        </svg>
        """)
end

function Base.show(io::IO, ::MIME"image/svg+xml",
                   cs::AbstractVector{T}) where T <: Color
    show(io, MIME"image/svg+xml"(), Base.ReshapedArray(cs, (1, length(cs)), ()))
end

function Base.show(io::IO, ::MIME"image/svg+xml",
                           cs::AbstractMatrix{T}) where T <: Color
    m, n = size(cs)
    apsect_ratio = clamp(n / m,
                         max_pixel_size / max_height,
                         max_width / max_pixel_size)
    pixel_aspect = apsect_ratio / (n / m)
    scale_factor = min(max_width / (n * max_pixel_size * pixel_aspect),
                       max_height / (m * max_pixel_size),
                       1)
    ysize = max_pixel_size * scale_factor
    xsize = ysize * pixel_aspect

    xpad = n > 50 ? 0 : 1
    ypad = m > 28 ? 0 : 1

    write(io,
        """
        <?xml version="1.0" encoding="UTF-8"?>
        <!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN"
         "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
        <svg xmlns="http://www.w3.org/2000/svg" version="1.1"
             width="$(n*xsize)mm" height="$(m*ysize)mm"
             shape-rendering="crispEdges">
        """)

    for i in 1:m, j in 1:n
        c = ndims(cs) == 2 ? cs[i, j] : cs[j]
        write(io,
            """
            <rect x="$((j-1)*xsize)mm" y="$((i-1)*ysize)mm"
                  width="$(xsize - xpad)mm" height="$(ysize - ypad)mm"
                  fill="#$(hex(c))" stroke="none" />
            """)
    end

    write(io, "</svg>")
end


using Test
using MPI

if get(ENV,"JULIA_MPI_TEST_ARRAYTYPE","") == "CuArray"
    import CUDA
    ArrayType = CUDA.CuArray
else
    ArrayType = Array
end

MPI.Init()

comm = MPI.COMM_WORLD
root = 0
rank = MPI.Comm_rank(comm)
sz   = MPI.Comm_size(comm)
isroot = MPI.Comm_rank(comm) == root

for T in Base.uniontypes(MPI.MPIDatatype)

    # Allocating
    A = ArrayType{T}(fill(T(rank+1), 4))
    C = MPI.Gather(A, root, comm)
    if isroot
        @test C isa ArrayType{T}
        @test C == ArrayType{T}(repeat(1:sz, inner=4))
    end

    # Allocating, object
    C = MPI.Gather(T(rank+1), root, comm)
    if isroot
        @test C isa Vector{T}
        @test C == Vector{T}(1:sz)
    end

    # Allocating, explicit length
    A = ArrayType{T}([MPI.Comm_rank(comm) + 1, 0])
    C = MPI.Gather(A, 1, root, comm)
    if isroot
        @test C isa ArrayType{T}
        @test C == ArrayType{T}(1:MPI.Comm_size(comm))
    end

    # Allocating, view
    A = ArrayType{T}([MPI.Comm_rank(comm) + 1, 0])
    C = MPI.Gather(view(A, 1:1), root, comm)
    if isroot
        @test C isa ArrayType{T}
        @test C == ArrayType{T}(1:MPI.Comm_size(comm))
    end

    # Non Allocating
    A = ArrayType{T}(fill(T(rank+1), 4))
    C = ArrayType{T}(undef, 4sz)
    MPI.Gather!(A, C, length(A), root, comm)
    if isroot
        @test C == ArrayType{T}(repeat(1:sz, inner=4))
    end

    # In_place
    A = ArrayType{T}(fill(T(rank+1), 4))
    if isroot
        A = ArrayType{T}(fill(T(rank+1), 4*sz))
    end
    if root == MPI.Comm_rank(comm)
        MPI.Gather!(nothing, A, 4, root, comm)
    else
        MPI.Gather!(A, nothing, 4, root, comm)
    end
    if isroot
        @test A == ArrayType{T}(repeat(1:sz, inner=4))
    end

end

MPI.Finalize()
@test MPI.Finalized()


# This code is part of QuantumCircuits.
#
# (C) Copyright Rafał Pracht 2022.
#
# This code is licensed under the Apache License, Version 2.0. You may
# obtain a copy of this license in the LICENSE.txt file in the root directory
# of this source tree or at http://www.apache.org/licenses/LICENSE-2.0.
#
# Any modifications or derivative works of this code must retain this
# copyright notice, and modified files need to carry a notice indicating
# that they have been altered from the originals.

module Qiskit

using PyCall

using QuantumCircuits.QCircuits.QBase
using QuantumCircuits.QCircuits.Gates
using QuantumCircuits.QCircuits.OtherGates
using QuantumCircuits.QCircuits.Instructions
using QuantumCircuits.QCircuits.Registers

import QuantumCircuits.QCircuits.QBase: add!, tomatrix, measure!
import Base.show

export QiskitSimulator, QiskitCircuit, getQRegister

const qiskit = PyNULL() # pyimport("qiskit")
const plt = PyNULL()
IS_QISKIT = false

function __init__()
    try
        copy!(qiskit, pyimport("qiskit"))
        copy!(plt, pyimport("matplotlib.pyplot"))

        # imports
        pyimport("qiskit.ignis")
        pyimport("qiskit.ignis.verification")

        global IS_QISKIT = true
    catch e
        println(e)
        println("ERROR: QuantumCircuits need qiskit, qiskit.ignis and matplotlib python package.")
    end
end

"The error throw when there is no qiskit instaled."
struct NoQiskitError <:Exception
    msg::String
end

"Quantum device provided by Qiskit."
abstract type QiskitDevice <: QuantumDevice end

"Nothing function"
const nop = () -> nothing

"Quantum circuit provided by Qiskit."
mutable struct QiskitCircuit <: QuantumCircuit
    qc::PyObject
    qubits::Dict{Qubit, PyObject}
    cbits::Dict{Cbit, PyObject}
    qRegs::Dict{String, PyObject}
    cRegs::Dict{String, PyObject}
    draw::Function

    function QiskitCircuit(qRegs::Vector{QuantumRegister},
                           cRegs::Union{Vector{ClassicalRegister}, Nothing} = nothing)
        qregs = [qiskit.QuantumRegister(length(r.bits), r.name) for r in qRegs]
        qubits = Dict{Qubit, PyObject}()
        for (qr, qqr) in zip(qRegs, qregs)
            for (k, v) in zip(qr, qqr)
                push!(qubits, k => v)
            end
        end
        qRegs = Dict{String, PyObject}()
        for r in qregs
            push!(qRegs, r.name => r)
        end

        if cRegs != nothing
            cregs = [qiskit.ClassicalRegister(length(r.bits), r.name) for r in cRegs]

            cbits = Dict{Cbit, PyObject}()
            for (cr, ccr) in zip(cRegs, cregs)
                for (k, v) in zip(cr, ccr)
                    push!(cbits, k => v)
                end
            end

            cRegs = Dict{String, PyObject}()
            for r in cregs
                push!(cRegs, r.name => r)
            end

            # , name="A"
            qc = new(qiskit.QuantumCircuit(qregs..., cregs...), qubits, cbits, qRegs, cRegs, nop)
        else
            cRegs = Dict{String, PyObject}()
            cbits = Dict{Cbit, PyObject}()
            qc = new(qiskit.QuantumCircuit(qregs...), qubits, cbits, qRegs, cRegs, nop)
        end

        qc.draw = () -> draw(qc)

        return qc
    end
end

"Draw the circuit."
function draw(qc::QiskitCircuit)
    qc.qc.draw()
    plt.show()
end

"Function return the register"
getQRegister(qc::QiskitCircuit, name::String) = qc.qRegs[name]

"Add quantum gate to Qiskit circuit"
addQiskitCode(qc::QiskitCircuit, gate::X) = qc.qc.x(qc.qubits[gate.qubit])
addQiskitCode(qc::QiskitCircuit, gate::Sx) = qc.qc.sx(qc.qubits[gate.qubit])
addQiskitCode(qc::QiskitCircuit, gate::Y) = qc.qc.y(qc.qubits[gate.qubit])
addQiskitCode(qc::QiskitCircuit, gate::Z) = qc.qc.z(qc.qubits[gate.qubit])
addQiskitCode(qc::QiskitCircuit, gate::H) = qc.qc.h(qc.qubits[gate.qubit])
addQiskitCode(qc::QiskitCircuit, gate::S) = qc.qc.s(qc.qubits[gate.qubit])
addQiskitCode(qc::QiskitCircuit, gate::Sd) = qc.qc.sdg(qc.qubits[gate.qubit])
addQiskitCode(qc::QiskitCircuit, gate::T) = qc.qc.t(qc.qubits[gate.qubit])
addQiskitCode(qc::QiskitCircuit, gate::Td) = qc.qc.tdg(qc.qubits[gate.qubit])
addQiskitCode(qc::QiskitCircuit, cx::CX) = qc.qc.cx(qc.qubits[cx.control], qc.qubits[cx.target])
addQiskitCode(qc::QiskitCircuit, gate::U3) = qc.qc.u3(getvalue(gate.θ), getvalue(gate.ϕ), getvalue(gate.λ), qc.qubits[gate.qubit])
addQiskitCode(qc::QiskitCircuit, gate::Rx) = qc.qc.rx(getvalue(gate.θ), qc.qubits[gate.qubit])
addQiskitCode(qc::QiskitCircuit, gate::Ry) = qc.qc.ry(getvalue(gate.θ), qc.qubits[gate.qubit])
addQiskitCode(qc::QiskitCircuit, gate::Rz) = qc.qc.rz(getvalue(gate.θ), qc.qubits[gate.qubit])
addQiskitCode(qc::QiskitCircuit, gate::Rzx) = qc.qc.rzx(getvalue(gate.θ), qc.qubits[gate.qubit1], qc.qubits[gate.qubit2])
addQiskitCode(qc::QiskitCircuit, gate::Barrier) = qc.qc.barrier()
function addQiskitCode(qc::QiskitCircuit, gate::U)
    qc.qc.rz(gate.β, qc.qubits[gate.qubit])
    qc.qc.ry(gate.γ, qc.qubits[gate.qubit])
    qc.qc.rz(gate.δ, qc.qubits[gate.qubit])
end

"Add gate to circuit"
function add!(qc::QiskitCircuit, gate::QuantumGate)
    addQiskitCode(qc, gate)
    nothing
end

function measure!(qc::QiskitCircuit, qubit::Qubit, cbit::Cbit)
    qc.qc.measure(qc.qubits[qubit], qc.cbits[cbit])
    nothing
end

"Qiskit simulator"
struct QiskitSimulator <: QiskitDevice
end

"Show method"
function Base.show(io::IO, qc::QiskitCircuit)
    s = string(qc.qc.draw(output="text"))
    print(io, s[10:end])
end

"Get the matrix version of the cirquit."
function tomatrix(qc::QiskitCircuit)
    backend = qiskit.BasicAer.get_backend("unitary_simulator")

    return qiskit.execute(qc.qc, backend).result().get_unitary()
end

end  # module Qiskit


#****************************************************************************
#                     Molecular Dynamics Potentials (MDP)
#                            CESMIX-MIT Project
#
#  Contributing authors: Ngoc-Cuong Nguyen (cuongng@mit.edu, exapde@gmail.com)
#****************************************************************************

function genpotential(foldername, filename, u, xij, xik, xil, xi, xj, xk, xl, qi, qj, qk, ql, ti, tj, tk, tl, ai, aj, ak, al, mu, eta, kappa, pot)

n = length(u);
gen = 1;
ifile = 0;
stropu = "";
strcpu = "";
strgpu = "";
potnum = 0;

for i = 1:n
    ui = u[i];
    if ui != 0
        potnum = potnum + 1;
        fn = filename * string(potnum);
        if pot==1
            tmpopu, tmpcpu, tmpgpu = gensingle(foldername, fn, ui, xi, qi, ti, ai, mu, eta, kappa, gen, ifile);
        elseif pot==2
            tmpopu, tmpcpu, tmpgpu = genpair(foldername, fn, ui, xij, qi, qj, ti, tj, ai, aj, mu, eta, kappa, gen, ifile);
        elseif pot==3
            tmpopu, tmpcpu, tmpgpu = gentriplet(foldername, fn, ui, xij, xik, qi, qj, qk, ti, tj, tk, ai, aj, ak, mu, eta, kappa, gen, ifile);
        elseif pot==4
            tmpopu, tmpcpu, tmpgpu = genquadruplet(foldername, fn, ui, xij, xik, xil, qi, qj, qk, ql, ti, tj, tk, tl, ai, aj, ak, al, mu, eta, kappa, gen, ifile);
        end
        tmpopu = modifystr1(tmpopu);
        tmpcpu = modifystr1(tmpcpu);
        tmpgpu = modifystr1(tmpgpu);
        stropu = stropu * tmpopu * "\n";
        strcpu = strcpu * tmpcpu * "\n";
        strgpu = strgpu * tmpgpu * "\n";
    end
end

if potnum==0
    nopotential(foldername, filename, u, xij, xik, xil, xi, xj, xk, xl, qi, qj, qk, ql, ti, tj, tk, tl, ai, aj, ak, al, mu, eta, kappa, pot);
else
    sp0, sp1, sp2, sp3 = getpotstr(pot);
    sp0 = replace(sp0, "int ng)" => "int ng, int potnum)");
    sp1 = replace(sp1, "int);" => "int, int);");
    sp2 = replace(sp2, "int);" => "int, int);");
    tmpopu, tmpcpu, tmpgpu = genpotnum(filename, potnum, sp0, sp1, sp2, sp3);
    stropu = stropu * tmpopu;
    strcpu = strcpu * tmpcpu;
    strgpu = strgpu * tmpgpu;

    # here
    if pot==1
        tmpgpu = replace(tmpgpu, "(T *u," => "Gradient(T *u, T *du, T *u_xi,");
        tmpgpu = replace(tmpgpu, "(u," => "Gradient(u, du, u_xi,");
        tmpgpu = replace(tmpgpu, "(double *" => "Gradient(double *, double *, double*");
        tmpgpu = replace(tmpgpu, "(float *" => "Gradient(float *, float *, float*");
    elseif pot==2
        tmpgpu = replace(tmpgpu, "(T *u," => "Gradient(T *u, T *du, T *u_xij,");
        tmpgpu = replace(tmpgpu, "(u," => "Gradient(u, du, u_xij,");
        tmpgpu = replace(tmpgpu, "(double *" => "Gradient(double *, double *, double*");
        tmpgpu = replace(tmpgpu, "(float *" => "Gradient(float *, float *, float*");
    elseif pot==3
        tmpgpu = replace(tmpgpu, "(T *u," => "Gradient(T *u, T *du, T *u_xij, T *u_xik,");
        tmpgpu = replace(tmpgpu, "(u," => "Gradient(u, du, u_xij, u_xik,");
        tmpgpu = replace(tmpgpu, "(double *" => "Gradient(double *, double *, double*, double*");
        tmpgpu = replace(tmpgpu, "(float *" => "Gradient(float *, float *, float*, float*");
    elseif pot==4
        tmpgpu = replace(tmpgpu, "(T *u," => "Gradient(T *u, T *du, T *u_xij, T *u_xik, T *u_xil,");
        tmpgpu = replace(tmpgpu, "(u," => "Gradient(u, du, u_xij, u_xik, u_xil,");
        tmpgpu = replace(tmpgpu, "(double *" => "Gradient(double *, double *, double*, double*, double*");
        tmpgpu = replace(tmpgpu, "(float *" => "Gradient(float *, float *, float*, float*, float*");
    end
    #strgpu = strgpu * "\n" * tmpgpu;
    strgpu = strgpu * "\n" * "#ifdef _ENZYME\n" * tmpgpu * "#endif\n";

    cppfiles(foldername, filename, stropu, strcpu, strgpu, 0);
end

end

function modifystr1(oldstr)

newstr = oldstr;
s = "u[" * string(0) * " + i*" * string(1) * "]";
if contains(newstr, s)
    news = "u[i]";
    newstr = replace(newstr, s => news);
end

return newstr

end


"""
Creates a python file that can be run in Blender to generate a
representation of an object created by the CGinJulia package

'''Bl_Export(filename::String, points::AbstractVector{Point3H}, faces::AbstractVector{Real})'''
"""
function Bl_Export(filename::String,points,faces)

io = open(filename, "w")

Code::String = """bl_info = {
    "name": "Convex Hull Generation in Julia",
    "author": "Jackson Meade (NCSSM) and Jack Snoeyink (UNC Chapel Hill)",
    "version": (1, 0),
    "blender": (2, 80, 0),
    "location": "View3D > Add > Mesh > CGinJulia Convex Hull",
    "description": "Adds a new Mesh Object",
    "warning": "Please make sure you are familiar with the CGinJulia package before using this EXPERIMENTAL add-on",
    "wiki_url": "github.com/snoeyink/CGinJulia",
    "category": "Add Convex Hull",
}

import bpy
from bpy.types import Operator
from bpy.props import FloatVectorProperty
from bpy_extras.object_utils import AddObjectHelper, object_data_add
from mathutils import Vector

def add_object(self, context):
    scale_x = self.scale.x
    scale_y = self.scale.y
    scale_z = self.scale.z

    verts = ["""

for i = 1:length(points)
    Code = string(Code,string("""Vector((""",points[i][2],""" * scale_x""",""",""",points[i][3],""" * scale_y""",""",""",points[i][4],""" * scale_z""",""")),""","""
    """))
end

Code = string(Code,"""]
""","""    edges = []
""","""    faces = [""")

for a in faces
    Code = string(Code,string("""[""",string(a[1]),""",""",string(a[2]),""",""",string(a[3]),"""],"""))
end

Code = string(Code,"""]
""","""    mesh = bpy.data.meshes.new(name="Convex Hull of """,filename,"""")
    mesh.from_pydata(verts, edges, faces)
    # useful for development when the mesh may be invalid.
    # mesh.validate(verbose=True)
    object_data_add(context, mesh, operator=self)

class OBJECT_OT_add_object(Operator, AddObjectHelper):
    """,'\u0022','\u0022','\u0022',"""Generate the Mesh Object from Points and CGinJulia-Constructed Faces""",'\u0022','\u0022','\u0022',"""

    bl_idname = "mesh.add_object"
    bl_label = "Show Representation"
    bl_options = {'REGISTER', 'UNDO'}

    scale: FloatVectorProperty(
        name="scale",
        default=(1.0, 1.0, 1.0),
        subtype='TRANSLATION',
        description="scaling",
    )

    def execute(self, context):

        add_object(self, context)

        return {'FINISHED'}

# Registration

def add_object_button(self, context):
    self.layout.operator(
        OBJECT_OT_add_object.bl_idname,
        text="CGinJulia Convex Hull (""",filename,""")",
        icon='SURFACE_NSPHERE')

# This allows you to right click on a button and link to the manual
def add_object_manual_map():
    url_manual_prefix = "https://docs.blender.org/manual/en/dev/"
    url_manual_mapping = (
        ("bpy.ops.mesh.add_object", "editors/3dview/object"),
    )
    return url_manual_prefix, url_manual_mapping

def register():
    bpy.utils.register_class(OBJECT_OT_add_object)
    bpy.utils.register_manual_map(add_object_manual_map)
    bpy.types.VIEW3D_MT_mesh_add.append(add_object_button)

def unregister():
    bpy.utils.unregister_class(OBJECT_OT_add_object)
    bpy.utils.unregister_manual_map(add_object_manual_map)
    bpy.types.VIEW3D_MT_mesh_add.remove(add_object_button)

if __name__ == "__main__":
    register()
""")

write(io, Code)

close(io)

end


"""
    VTKCellTypes

Module defining cell types for unstructured datasets.

Definitions are adapted from the [`VTK source
code`](https://raw.githubusercontent.com/Kitware/VTK/master/Common/DataModel/vtkCellType.h).
"""
module VTKCellTypes

export VTKCellType, nodes

struct VTKCellType
    vtk_name::String
    vtk_id::UInt8
    nodes::Int
end

"""
    nodes(c::VTKCellTypes)

Returns the number of nodes (or grid points) required by the cell type.

For instance, this returns 3 for `VTK_TRIANGLE`.

For cell types that can take any number of nodes, such as `VTK_POLY_LINE`,
this returns -1.
"""
nodes(c::VTKCellType) = c.nodes

const INVALID_CELL_TYPE = VTKCellType("INVALID", typemax(UInt8), -1)

# Array of known cell types
const VTK_CELL_TYPES = fill(INVALID_CELL_TYPE, typemax(UInt8))

# Construction of VTKCellType objects from the integer vtk id
function VTKCellType(vtkid::Integer)
    n = vtkid + 1  # ids start at zero
    cell = VTK_CELL_TYPES[n]
    if cell === INVALID_CELL_TYPE
        throw(ArgumentError("unknown vtkid = $vtkid"))
    end
    cell
end

# Define a VTK cell type
macro add(ctype, id, nodes=-1)
    name = string(ctype)
    quote
        const $(esc(ctype)) = VTKCellType($name, $id, $nodes)
        VTK_CELL_TYPES[$id + 1] = $(esc(ctype))
    end
end

# Linear cells
@add VTK_EMPTY_CELL 0 0
@add VTK_VERTEX 1  1
@add VTK_POLY_VERTEX 2 -1
@add VTK_LINE 3  2
@add VTK_POLY_LINE 4 -1
@add VTK_TRIANGLE 5  3
@add VTK_TRIANGLE_STRIP 6 -1
@add VTK_POLYGON 7 -1
@add VTK_PIXEL 8  4
@add VTK_QUAD 9  4
@add VTK_TETRA 10 4
@add VTK_VOXEL 11 8
@add VTK_HEXAHEDRON 12 8
@add VTK_WEDGE 13 6
@add VTK_PYRAMID 14 5
@add VTK_PENTAGONAL_PRISM 15 10
@add VTK_HEXAGONAL_PRISM 16 12

# Quadratic, isoparametric cells
@add VTK_QUADRATIC_EDGE 21 3
@add VTK_QUADRATIC_TRIANGLE 22 6
@add VTK_QUADRATIC_QUAD 23 8
@add VTK_QUADRATIC_POLYGON 36 -1
@add VTK_QUADRATIC_TETRA 24 10
@add VTK_QUADRATIC_HEXAHEDRON 25 20
@add VTK_QUADRATIC_WEDGE 26 15
@add VTK_QUADRATIC_PYRAMID 27 13
@add VTK_BIQUADRATIC_QUAD 28 9
@add VTK_TRIQUADRATIC_HEXAHEDRON 29 27
@add VTK_QUADRATIC_LINEAR_QUAD 30 6
@add VTK_QUADRATIC_LINEAR_WEDGE 31 12
@add VTK_BIQUADRATIC_QUADRATIC_WEDGE 32 18
@add VTK_BIQUADRATIC_QUADRATIC_HEXAHEDRON 33 24
@add VTK_BIQUADRATIC_TRIANGLE 34 7

# Cubic, isoparametric cell
@add VTK_CUBIC_LINE 35 4

# Special class of cells formed by convex group of points
@add VTK_CONVEX_POINT_SET 41

# Polyhedron cell (consisting of polygonal faces)
@add VTK_POLYHEDRON 42

# Higher order cells in parametric form
@add VTK_PARAMETRIC_CURVE 51
@add VTK_PARAMETRIC_SURFACE 52
@add VTK_PARAMETRIC_TRI_SURFACE 53
@add VTK_PARAMETRIC_QUAD_SURFACE 54
@add VTK_PARAMETRIC_TETRA_REGION 55
@add VTK_PARAMETRIC_HEX_REGION 56

# Higher order cells
@add VTK_HIGHER_ORDER_EDGE 60
@add VTK_HIGHER_ORDER_TRIANGLE 61
@add VTK_HIGHER_ORDER_QUAD 62
@add VTK_HIGHER_ORDER_POLYGON 63
@add VTK_HIGHER_ORDER_TETRAHEDRON 64
@add VTK_HIGHER_ORDER_WEDGE 65
@add VTK_HIGHER_ORDER_PYRAMID 66
@add VTK_HIGHER_ORDER_HEXAHEDRON 67

# Arbitrary order Lagrange elements (formulated separated from generic higher order cells)
@add VTK_LAGRANGE_CURVE 68
@add VTK_LAGRANGE_TRIANGLE 69
@add VTK_LAGRANGE_QUADRILATERAL 70
@add VTK_LAGRANGE_TETRAHEDRON 71
@add VTK_LAGRANGE_HEXAHEDRON 72
@add VTK_LAGRANGE_WEDGE 73
@add VTK_LAGRANGE_PYRAMID 74

# Arbitrary order Bezier elements (formulated separated from generic higher order cells)
@add VTK_BEZIER_CURVE 75
@add VTK_BEZIER_TRIANGLE 76
@add VTK_BEZIER_QUADRILATERAL 77
@add VTK_BEZIER_TETRAHEDRON 78
@add VTK_BEZIER_HEXAHEDRON 79
@add VTK_BEZIER_WEDGE 80
@add VTK_BEZIER_PYRAMID 81

end # module


# This file is a part of JuliaFEM.
# License is MIT: see https://github.com/JuliaFEM/MortarContact2D.jl/blob/master/LICENSE

using FEMBase, MortarContact2D, Test
using MortarContact2D: create_contact_segmentation, calculate_normals

X = Dict(1 => [0.0, 0.0],
         2 => [1.0, 0.0],
         3 => [0.0, 1.0],
         4 => [1.0, 1.0])

slave = Element(Seg2, [1, 2])
master = Element(Seg2, [3, 4])
elements = [slave, master]
update!(elements, "geometry", X)
update!(slave, "displacement", 0.0 => ([0.0,0.0], [0.0,0.0]))
update!(master, "displacement", 0.0 => ([0.0,0.0], [0.0,0.0]))
update!(slave, "displacement", 1.0 => ([0.0,0.0], [0.0,0.0]))
update!(master, "displacement", 1.0 => ([2.0,0.0], [2.0,0.0]))
problem = Problem(Contact2D, "test problem", 2, "displacement")
add_slave_elements!(problem, [slave])
add_master_elements!(problem, [master])
normals = calculate_normals([slave], 0.0)
update!([slave], "normal", 0.0 => normals)
seg1 = create_contact_segmentation(problem, slave, [master], 0.0; deformed=true)
seg2 = create_contact_segmentation(problem, slave, [master], 1.0; deformed=true)
problem.properties.max_distance = 0.0
seg3 = create_contact_segmentation(problem, slave, [master], 0.0; deformed=false)
@test length(seg1) == 1
@test length(seg2) == 0
@test length(seg3) == 0

slave = Element(Seg2, [1, 2])
master = Element(Seg2, [3, 4])
elements = [slave, master]
update!(elements, "geometry", X)
problem = Problem(Contact2D, "test problem", 2, "displacement")
problem.assembly.la = zeros(8)
problem.properties.store_fields = [
    "contact area",
    "contact error",
    "weighted gap",
    "contact pressure",
    "complementarity condition",
    "active nodes",
    "inactive nodes",
    "stick nodes",
    "slip nodes"]
add_slave_elements!(problem, [slave])
add_master_elements!(problem, [master])
assemble!(problem, 0.0)

X[3] += [2.0, 0.0]
X[4] += [2.0, 0.0]
slave = Element(Seg2, [1, 2])
master = Element(Seg2, [3, 4])
elements = [slave, master]
update!(elements, "geometry", X)
problem = Problem(Contact2D, "test problem", 2, "displacement")
add_slave_elements!(problem, [slave])
add_master_elements!(problem, [master])
assemble!(problem, 0.0)

X = Dict(1 => [0.0, 0.0],
         2 => [1.0, 0.0],
         3 => [0.0, 0.0],
         4 => [1.0, 0.0])

slave = Element(Seg2, [1, 2])
master = Element(Seg2, [3, 4])
elements = [slave, master]
update!(elements, "geometry", X)
problem = Problem(Contact2D, "test problem", 2, "displacement")
add_slave_elements!(problem, [slave])
add_master_elements!(problem, [master])
assemble!(problem, 0.0)
empty!(problem.assembly)
problem.properties.iteration = 0
problem.properties.contact_state_in_first_iteration = :INACTIVE
assemble!(problem, 0.0)


# Helper... Refactoring needed.
function getfieldinterpolation(dh::Ferrite.DofHandler, field_name::Symbol)
    field_idx = indexin(dh.field_names, [:b])
    field_idx == nothing && error("did not find field $field_name")
    dh.field_interpolations[field_idx]
end

function field_offset(dh::Ferrite.DofHandler, field_idx::Int)
    offset = 0
    for i in 1:(field_idx-1)
        offset += Ferrite.getnbasefunctions(dh.field_interpolations[i])::Int * dh.field_dims[i]
    end
    return offset
end

function facedofs(cell, local_face_idx::Int, field_idx::Int)
    celldofs_ = Ferrite.celldofs(cell)
    offset_ = field_offset(cell.dh, field_idx)
    fip = Ferrite.getfieldinterpolation(cell.dh, field_idx)
    faces_ = Ferrite.faces(fip)
    isempty(faces_) && return () # no face dofs :)
    indices = faces_[local_face_idx] .+ offset_
    celldofs_[[indices...]]
end

Ferrite.vertices(cell::Ferrite.Cell{3,3,1}) = cell.nodes

Ferrite.default_interpolation(::Type{Ferrite.Cell{3,3,1}}) = Ferrite.Lagrange{2,Ferrite.RefTetrahedron,1}()

# Note: This extracts the face spanned by the vertices, not the actual face!
linear_face_cell(cell::Ferrite.Cell{3,N,4}, local_face_idx::Int) where N =  Ferrite.Cell{3,3,1}(Ferrite.faces(cell)[local_face_idx])
linear_face_cell(cell::Ferrite.Cell{3,N,6}, local_face_idx::Int) where N = Ferrite.Quadrilateral3D(Ferrite.faces(cell)[local_face_idx])

# Obtain the face interpolation on regular geometries.
getfaceip(ip::Ferrite.Interpolation{dim, shape, order}, local_face_idx::Int) where {dim, shape <: Union{Ferrite.RefTetrahedron, Ferrite.RefCube}, order} = Ferrite.getlowerdim(ip)

struct MakiePlotter{dim,DH<:Ferrite.AbstractDofHandler,T} <: AbstractPlotter
    dh::DH
    u::Makie.Observable{Vector{T}} # original solution on the original mesh (i.e. dh.mesh)

    cells_connectivity::Vector

    gridnodes::Matrix{AbstractFloat} # coordinates of grid nodes in matrix form
    physical_coords::Matrix{AbstractFloat} # coordinates in physical space of a vertex
    triangles::Matrix{Int} # each row carries a triple with the indices into the coords matrix defining the triangle
    reference_coords::Matrix{AbstractFloat} # coordinates on the reference cell for the corresponding vertex
end

function MakiePlotter(dh::Ferrite.AbstractDofHandler, u::Vector)
    cells = Ferrite.getcells(dh.grid)
    dim = Ferrite.getdim(dh.grid)

    # We do not take any assumptions on the mesh, so first we have to loopkup
    num_triangles = 0
    for cell in cells
        num_triangles += ntriangles(cell)
    end

    # Preallocate the matrices carrying the triangulation information
    triangles = Matrix{Int}(undef, num_triangles, 3)
    physical_coords = Matrix{Float64}(undef, num_triangles*3, dim)
    gridnodes = [node[i] for node in Ferrite.getcoordinates.(Ferrite.getnodes(dh.grid)), i in 1:dim]
    reference_coords = Matrix{Float64}(undef, num_triangles*3, 2) # for now no 1D

    # Decompose does the heavy lifting for us
    coord_offset = 1
    triangle_offset = 1
    for cell in cells
        (coord_offset, triangle_offset) = decompose!(coord_offset, physical_coords, reference_coords, triangle_offset, triangles, dh.grid, cell)
    end
    return MakiePlotter{dim,typeof(dh),eltype(u)}(dh,Observable(u),[],gridnodes,physical_coords,triangles,reference_coords);
end

"""
Total number of vertices
"""
num_vertices(p::MakiePlotter) = size(p.physical_coords,1)

#Visualization is just fancy triangle plotting, every element needs to be translatable to a triangle *sadnoises*
to_triangle(cell::Ferrite.AbstractCell{2,N,3}) where N = [Ferrite.vertices(cell)[1:3]]
to_triangle(cell::Ferrite.AbstractCell{3,N,4}) where N = [Ferrite.vertices(cell)[[1,2,3]], Ferrite.vertices(cell)[[1,2,4]], Ferrite.vertices(cell)[[2,3,4]], Ferrite.vertices(cell)[[1,4,3]]]
to_triangle(cell::Ferrite.AbstractCell{2,N,4}) where N = [Ferrite.vertices(cell)[[1,2,3]], Ferrite.vertices(cell)[[3,4,1]]]
to_triangle(cell::Ferrite.AbstractCell{3,N,6}) where N = [Ferrite.vertices(cell)[[1,2,3]], Ferrite.vertices(cell)[[3,4,1]],
                                                          Ferrite.vertices(cell)[[1,5,6]], Ferrite.vertices(cell)[[6,2,1]],
                                                          Ferrite.vertices(cell)[[2,6,7]], Ferrite.vertices(cell)[[7,3,2]],
                                                          Ferrite.vertices(cell)[[3,7,8]], Ferrite.vertices(cell)[[8,4,3]],
                                                          Ferrite.vertices(cell)[[1,4,8]], Ferrite.vertices(cell)[[8,5,1]],
                                                          Ferrite.vertices(cell)[[5,8,7]], Ferrite.vertices(cell)[[7,6,5]]]

"""
TODO this looks faulty...think harder.
"""
# Helper to count triangles e.g. for preallocations.
ntriangles(cell::Ferrite.AbstractCell{2,3,3}) where {N} = 1 # Tris in 2D
ntriangles(cell::Ferrite.AbstractCell{3,3,1}) where {N} = 1 # Tris in 3D
ntriangles(cell::Ferrite.AbstractCell{dim,N,4}) where {dim,N} = 4 # Quads in 2D and 3D
ntriangles(cell::Ferrite.AbstractCell{3,N,1}) where {dim,N} = 4 # Tets as a special case of a Quad, obviously :)
ntriangles(cell::Ferrite.AbstractCell{3,N,6}) where {dim,N} = 6*4 # Hex

"""
Get the vertices represented as a list of coordinates of a cell.

@TODO refactor into Ferrite core.
"""
function vertices(grid::Ferrite.AbstractGrid, cell::Ferrite.AbstractCell{dim,N,M}) where {dim,N,M}
    Ferrite.getnodes(grid)[[Ferrite.vertices(cell)...]]
end

"""
Decompose a triangle into a coordinates and a triangle index list to disconnect it properly. Guarantees to preserve orderings and orientations.
"""
function decompose!(coord_offset, coord_matrix, ref_coord_matrix, triangle_offset, triangle_matrix, grid, cell::Union{Ferrite.AbstractCell{2,N,3}, Ferrite.AbstractCell{3,3,1}}) where {N}
    for (i,v) in enumerate(vertices(grid, cell))
        coord_matrix[coord_offset, :] = Ferrite.getcoordinates(v)
        ref_coord_matrix[coord_offset, :] = Ferrite.reference_coordinates(Ferrite.default_interpolation(typeof(cell)))[i]
        triangle_matrix[triangle_offset, i] = coord_offset
        coord_offset+=1
    end
    triangle_offset+=1
    (coord_offset, triangle_offset)
end

"""
Decompose a tetrahedron into a coordinates and a triangle index list to disconnect it properly. Guarantees to preserve orderings and orientations.
"""
function decompose!(coord_offset, coord_matrix, ref_coord_matrix, triangle_offset, triangle_matrix, grid, cell::Ferrite.AbstractCell{3,N,4}) where {N}
    # Is just 4 triangles :)
    for ti ∈ Ferrite.faces(cell)
        (coord_offset, triangle_offset) = decompose!(coord_offset, coord_matrix, ref_coord_matrix, triangle_offset, triangle_matrix, grid, Ferrite.Cell{3,3,1}(ti))
    end
    (coord_offset, triangle_offset)
end

"""
Decompose a quadrilateral into a coordinates and a triangle index list to disconnect it properly. Guarantees to preserve orderings and orientations.

Assumes a CCW quadrilateral numbering:
4---3
|   |
1---2
and creates the decomposition
4-------3
| \\ C / |
|  \\ /  |
|D  5  B|
|  / \\  |
| / A \\ |
1-------2
where A=(1,2,5),B=(2,3,5),C=(3,4,5),D=(4,1,5) are the generated triangles in this order.
"""
function decompose!(coord_offset, coord_matrix, ref_coord_matrix, triangle_offset, triangle_matrix, grid, cell::Union{Ferrite.AbstractCell{2,N,4}, Ferrite.AbstractCell{3,4,1}}) where {N}
    # A bit more complicated. The default diagonal decomposition into 2 triangles is missing a solution mode.
    # To resolve this we make a more expensive decomposition in 4 triangles which correctly displays the solution mode (in linear problems).
    coord_offset_initial = coord_offset
    vts = vertices(grid, cell)
    space_dim = size(coord_matrix,2)

    # Compute coordinate of vertex 5
    center = [ntuple(x->0.0, space_dim)...]
    for v in vts
        center += Ferrite.getcoordinates(v)
    end
    center /= 4.0

    # Generate triangles in order
    for i = 1:length(vts)
        v1 = vts[i]
        # next index on the outer chain CCW
        i2 = i+1
        if i2 > length(vts)
            i2 = 1 # dunno how to modulo this correctly :/
        end
        v2 = vts[i2]

        # current vertex
        coord_matrix[coord_offset, :] = Ferrite.getcoordinates(v1)
        ref_coord_matrix[coord_offset, :] = Ferrite.reference_coordinates(Ferrite.default_interpolation(typeof(cell)))[i]
        coord_offset+=1

        # next vertex in chain
        coord_matrix[coord_offset, :] = Ferrite.getcoordinates(v2)
        ref_coord_matrix[coord_offset, :] = Ferrite.reference_coordinates(Ferrite.default_interpolation(typeof(cell)))[i2]
        coord_offset+=1

        # center vertex (5)
        coord_matrix[coord_offset, :] = center
        ref_coord_matrix[coord_offset, :] = [ntuple(x->0.0, 2)...]
        coord_offset+=1

        # collect indices
        triangle_matrix[triangle_offset, :] = (0:2) .+ (coord_offset_initial+3*(i-1))
        triangle_offset+=1
    end

    (coord_offset, triangle_offset)
end

"""
Decompose a hexahedron into a coordinates and a triangle index list to disconnect it properly. Guarantees to preserve orderings and orientations.
"""
function decompose!(coord_offset, coord_matrix, ref_coord_matrix, triangle_offset, triangle_matrix, grid, cell::Ferrite.AbstractCell{3,N,6}) where {N}
    # Just 6 quadrilaterals :)
    for ti ∈ Ferrite.faces(cell)
        (coord_offset, triangle_offset) = decompose!(coord_offset, coord_matrix, ref_coord_matrix, triangle_offset, triangle_matrix, grid, Ferrite.Cell{3,4,1}(ti))
    end
    (coord_offset, triangle_offset)
end

refshape(cell::Ferrite.AbstractCell) = typeof(Ferrite.default_interpolation(typeof(cell))).parameters[2]

x₁(x) = x[1]
x₂(x) = x[2]
x₃(x) = x[3]
l2(x) = LinearAlgebra.norm(x,2)
l1(x) = LinearAlgebra.norm(x,1)

midpoint(cell::Ferrite.AbstractCell{2,N,3}, points) where N = Point2f((1/3) * (points[cell.nodes[1],:] + points[cell.nodes[2],:] + points[cell.nodes[3],:]))
midpoint(cell::Ferrite.AbstractCell{2,N,4}, points) where N = Point2f(0.5 * (points[cell.nodes[1],:] + points[cell.nodes[3],:]))
midpoint(cell::Ferrite.AbstractCell{3,N,4}, points) where N = Point3f((1/4) * (points[cell.nodes[1],:] + points[cell.nodes[2],:] + points[cell.nodes[3],:] + points[cell.nodes[4],:]))
midpoint(cell::Ferrite.AbstractCell{3,N,6}, points) where N = Point3f(0.5 * (points[cell.nodes[1],:] + points[cell.nodes[7],:]))

function postprocess(node_values)
    dim = length(node_values)
    if dim == 1
        return node_values
    else
        return sqrt(sum(node_values.^2))
    end
end

"""
Transfer the solution of a plotter to the new mesh in 2D.

@precondition: Assumes that the number of basis function for each dof is equal.

@TODO Refactor. This is peak inefficiency.
"""
function transfer_solution(plotter::MakiePlotter{2}, u::Vector; field_idx::Int=1, process::Function=FerriteViz.postprocess)
    n_vertices_per_tri = 3 # we have 3 vertices per triangle...

    # select objects from plotter
    dh = plotter.dh
    ref_coords = plotter.reference_coords
    grid = dh.grid

    # field related variables
    field_name = Ferrite.getfieldnames(dh)[field_idx]
    field_dim = Ferrite.getfielddim(dh, field_idx)
    ip = dh.field_interpolations[field_idx]

    # actual data
    local_dof_range = Ferrite.dof_range(dh, field_name)

    data = fill(0.0, num_vertices(plotter), field_dim)
    current_vertex_index = 1
    for (cell_index, cell) in enumerate(Ferrite.getcells(plotter.dh.grid))
        cell_geo = Ferrite.getcells(dh.grid,cell_index)
        _celldofs_field = reshape(Ferrite.celldofs(dh,cell_index)[local_dof_range], (field_dim, Ferrite.getnbasefunctions(ip)))

        # Loop over all local triangle vertices
        for i in 1:(ntriangles(cell_geo)*n_vertices_per_tri)
            ξ = Tensors.Vec(ref_coords[current_vertex_index, :]...)
            for d in 1:field_dim
                for node_idx ∈ 1:Ferrite.getnbasefunctions(ip)
                    data[current_vertex_index, d] += Ferrite.value(ip, node_idx, ξ) ⋅ u[_celldofs_field[d, node_idx]]
                end
            end
            current_vertex_index += 1
        end
    end
    return mapslices(process, data, dims=[2])
end

"""
Transfer the solution of a plotter to the new mesh in 3D. We just evaluate the faces.

@TODO Refactor. This is peak inefficiency.
"""
function transfer_solution(plotter::MakiePlotter{3}, u::Vector; field_idx::Int=1, process::Function=FerriteViz.postprocess) where T
    n_vertices_per_tri = 3 # we have 3 vertices per triangle...

    # select objects from plotter
    dh = plotter.dh
    ref_coords = plotter.reference_coords
    grid = dh.grid

    # field related variables
    field_name = Ferrite.getfieldnames(dh)[field_idx]
    field_dim = Ferrite.getfielddim(dh, field_idx)

    # @FIXME decouple the interpolation from the geometry, because for discontinuous interpolations
    #   the "interpolation faces" (which we need for dof-assignment) do not coincide with the geometric
    #   faces... Alternatively we could try something similar to FaceValues.
    ip_cell = dh.field_interpolations[field_idx]
    _faces = Ferrite.faces(ip_cell) # faces of the cell with local dofs
    @assert !isempty(_faces) # Discontinuous interpolations in 3d not supported yet. See above.

    # actual data
    local_dof_range = Ferrite.dof_range(dh, field_name)

    current_vertex_index = 1
    data = fill(0.0, num_vertices(plotter), field_dim)
    for (cell_index, cell) in enumerate(Ferrite.getcells(plotter.dh.grid))
        cell_geo = Ferrite.getcells(dh.grid,cell_index)

        _local_celldofs = Ferrite.celldofs(dh,cell_index)[local_dof_range]
        _celldofs_field = reshape(_local_celldofs, (field_dim, Ferrite.getnbasefunctions(ip_cell)))

        for (local_face_idx,_) in enumerate(Ferrite.faces(cell_geo))
            ip_face = getfaceip(ip_cell, local_face_idx)
            face_geo = linear_face_cell(cell_geo, local_face_idx)

            # extract face dofs
            _facedofs_field = _celldofs_field[:,[_faces[local_face_idx]...]]

            # Loop over vertices
            for i in 1:(ntriangles(face_geo)*n_vertices_per_tri)
                ξ = Tensors.Vec(ref_coords[current_vertex_index, :]...)
                for d in 1:field_dim
                    for node_idx ∈ 1:Ferrite.getnbasefunctions(ip_face)
                        data[current_vertex_index, d] += Ferrite.value(ip_face, node_idx, ξ) ⋅ u[_facedofs_field[d, node_idx]]
                    end
                end
                current_vertex_index += 1
            end
        end
    end

    return mapslices(process, data, dims=[2])
end

function transfer_scalar_celldata(plotter::MakiePlotter{3}, u::Vector; process::Function=FerriteViz.postprocess)
    n_vertices = 3 # we have 3 vertices per triangle...

    # select objects from plotter
    dh = plotter.dh
    grid = dh.grid

    current_vertex_index = 1
    data = fill(0.0, num_vertices(plotter), 1)
    for (cell_index, cell) in enumerate(Ferrite.getcells(grid))
        cell_geo = grid.cells[cell_index]
        for (local_face_idx,_) in enumerate(Ferrite.faces(cell_geo))
            face_geo = linear_face_cell(cell_geo, local_face_idx)
            # Loop over vertices
            for i in 1:(ntriangles(face_geo)*n_vertices)
                data[current_vertex_index, 1] = u[cell_index]
                current_vertex_index += 1
            end
        end
    end

    return mapslices(process, data, dims=[2])
end

function transfer_scalar_celldata(plotter::MakiePlotter{2}, u::Vector;  process::Function=FerriteViz.postprocess)
    n_vertices = 3 # we have 3 vertices per triangle...

    # select objects from plotter
    dh = plotter.dh
    grid = dh.grid

    current_vertex_index = 1
    data = fill(0.0, num_vertices(plotter), 1)
    for (cell_index, cell) in enumerate(Ferrite.getcells(grid))
        cell_geo = grid.cells[cell_index]
        for i in 1:(ntriangles(cell_geo)*n_vertices)
            data[current_vertex_index, 1] = u[cell_index]
            current_vertex_index += 1
        end
    end

    return mapslices(process, data, dims=[2])
end

function transfer_scalar_celldata(grid::Ferrite.AbstractGrid{3}, num_vertices::Number, u::Vector; process::Function=FerriteViz.postprocess)
    n_vertices = 3 # we have 3 vertices per triangle...
    current_vertex_index = 1
    data = fill(0.0, num_vertices, 1)
    for (cell_index, cell) in enumerate(Ferrite.getcells(grid))
        cell_geo = grid.cells[cell_index]
        for (local_face_idx,_) in enumerate(Ferrite.faces(cell_geo))
            face_geo = linear_face_cell(cell_geo, local_face_idx)
            # Loop over vertices
            for i in 1:(ntriangles(face_geo)*n_vertices)
                data[current_vertex_index, 1] = u[cell_index]
                current_vertex_index += 1
            end
        end
    end
    return mapslices(process, data, dims=[2])
end

function transfer_scalar_celldata(grid::Ferrite.AbstractGrid{2}, num_vertices::Number, u::Vector;  process::Function=FerriteViz.postprocess)
    n_vertices = 3 # we have 3 vertices per triangle...
    current_vertex_index = 1
    data = fill(0.0, num_vertices, 1)
    for (cell_index, cell) in enumerate(Ferrite.getcells(grid))
        cell_geo = grid.cells[cell_index]
        for i in 1:(ntriangles(cell_geo)*n_vertices)
            data[current_vertex_index, 1] = u[cell_index]
            current_vertex_index += 1
        end
    end
    return mapslices(process, data, dims=[2])
end

function dof_to_node(dh::Ferrite.AbstractDofHandler, u::Array{T,1}; field::Int=1, process::Function=postprocess) where T
    fieldnames = Ferrite.getfieldnames(dh)
    field_dim = Ferrite.getfielddim(dh, field)
    data = fill(NaN, Ferrite.getnnodes(dh.grid), field_dim)
    offset = Ferrite.field_offset(dh, fieldnames[field])

    for cell in Ferrite.CellIterator(dh)
        _celldofs = Ferrite.celldofs(cell)
        counter = 1
        for node in cell.nodes
            for d in 1:field_dim
                data[node, d] = u[_celldofs[counter + offset]]
                counter += 1
            end
        end
    end
    return mapslices(process, data, dims=[2])
end