text:
"""
    MeshModificationModule

Module for mesh modification operations.
"""
module MeshModificationModule

using FinEtools.FTypesModule: FInt, FFlt, FCplxFlt, FFltVec, FIntVec, FFltMat, FIntMat, FMat, FVec, FDataDict
import FinEtools.FESetModule: FESet, count, boundaryconn, boundaryfe, updateconn!, connasarray, fromarray!
import FinEtools.FENodeSetModule: FENodeSet
import FinEtools.BoxModule: boundingbox, inflatebox!, intersectboxes, inbox
import FinEtools.MeshSelectionModule: connectednodes
using Base.Sort
using Base.Order
import LinearAlgebra: norm, svd, dot, eigen
import Random: randperm
using SparseArrays

"""
    interior2boundary(interiorconn::Array{Int, 2}, extractb::Array{Int, 2})

Extract the boundary connectivity from the connectivity of the interior.
"""
function interior2boundary(interiorconn::Array{Int, 2}, extractb::Array{Int, 2})
    hypf = interiorconn[:, extractb[1, :]]
    for i = 2:size(extractb, 1)
        hypf = vcat(hypf, interiorconn[:, extractb[i, :]])
    end
    return _myunique2(hypf);
end

"""
meshboundary(fes::T) where {T<:FESet}

Extract the boundary finite elements from a mesh.

Extract the finite elements of manifold dimension (n-1) from the
supplied finite element set of manifold dimension (n).
"""
function meshboundary(fes::T) where {T<:FESet}
    # Form all hyperfaces, non-duplicates are boundary cells
    hypf = boundaryconn(fes);    # get the connectivity of the boundary elements
    bdryconn = _myunique2(hypf);
    make = boundaryfe(fes);     # get the function that can make a boundary element
    return make(bdryconn);
end

function _mysortrows(A::FIntMat)
    # Sort the rows of A by sorting each column from back to front.

    m,n = size(A);

    indx =  zeros(FInt,m); sindx = zeros(FInt,m)
    for i=1:m
        indx[i]=i
    end
    nindx =  zeros(FInt,m);
    col = zeros(FInt,m)
    for c = n:-1:1
        for i=1:m
            col[i]=A[indx[i],c]
        end
        #Sorting a column vector is much faster than sorting a column matrix
        sindx=sortperm(col,alg=QuickSort);
        #sortperm!(sindx,col,alg=QuickSort); # available for 0.4, slightly faster
        #indx=indx[sindx] # saving allocations by using the below loops
        for i=1:m
            nindx[i]=indx[sindx[i]]
        end
        for i=1:m
            indx[i]=nindx[i]
        end
    end

    return A[indx,:]
end

function _mysortdim2!(A::FIntMat)
    # Sort each row  of A in ascending order.

    m,n = size(A);
    r = zeros(FInt,n)
   @inbounds for k = 1:m
        for i=1:n
            r[i]=A[k,i]
        end
        sort!(r);
        for i=1:n
            A[k,i]=r[i]
        end
    end
    return A
end

function  _myunique2(A::FIntVec)
    return _myunique2(reshape(A, length(A), 1))
end

function  _myunique2(A::FIntMat) # speeded up; now the bottleneck is _mysortrows
    #println("size(A)=$(size(A))")
    maxA=maximum(A[:])::FInt
    sA=deepcopy(A)
    #@time
    sA=_mysortdim2!(sA)::FIntMat;#this is fast
    #@time sA=sort(A,2,alg=QuickSort)::FIntMat;#this is slow
    sA= [sA broadcast(+, 1:size(A,1), maxA)]::FIntMat
    #@time
    sA =_mysortrows(sA); # this now takes the majority of time, but much less than the function below
    #@time sA  = sortrows(sA,alg=QuickSort);;#this is slow
    rix=sA[:,end];
    broadcast!(-, rix, rix, maxA)
    sA=sA[:,1:end-1];
    d=falses(size(sA,1)-1)
    for k=1:length(d)
        for m=1:size(sA,2)
            if sA[k,m]!=sA[k+1,m]
                d[k]=true;
                break;
            end
        end
    end
    #d=(sA[1:end-1,:].!=sA[2:end,:]); # element-wise comparison!
    ad=zeros(FInt,size(d,1)+1)
    ad[1]=1;
    for k=2:length(ad)
        for m=1:size(d,2)
            if d[k-1,m]!=0
                ad[k]=1;
                break;
            end
        end
    end
    #ad=map((x) -> (x?1:0),[true; any(d,2)]);
    iu=trues(length(ad))
    for k=1:(length(ad)-1)
        ad[k]=ad[k]+ad[k+1]
        iu[k]=(ad[k]>1)
    end
    ad[end]=ad[end]+1;
    iu[end]=(ad[end]>1)
    #iu =map((x) -> (x>1? true: false),(ad + [ad[2:end];1]));
    Out =A[rix[iu],:];
    return Out
end

# ### This code is correct, but very slow.
# function  _myunique1(A::FIntMat)
#     maxA=maximum(A[:])
#     sA=sort(A,2);# most time spent here
#     sA= [sA (1:size(A,1))+maxA]
#     sA  = sortrows(sA);;#and here
#     rix=sA[:,end]; rix=rix[:]-maxA;
#     sA=sA[:,1:end-1];
#     d=(sA[1:end-1,:].!=sA[2:end,:]); # element-wise comparison!
#     ad=map((x) -> (x ? 1 : 0),[true; any(d,2)]);
#     iu =map((x) -> (x>1 ? true: false),(ad + [ad[2:end];1]));
#     Out =A[rix[iu[:]],:];
#     return Out
# end

"""
    fusenodes(fens1::FENodeSet, fens2::FENodeSet, tolerance:: FFlt)

Fuse together nodes from two node sets.

Fuse two node sets. If necessary, by gluing together nodes located within
tolerance of each other. The two node sets, `fens1` and `fens2`,  are fused
together by merging the nodes that fall within a box of size `tolerance`. The
merged node set, `fens`, and the new  indexes of the nodes in the set `fens1`
are returned.

The set `fens2` will be included unchanged, in the same order,
in the node set `fens`.
The indexes of the node set `fens1` will have changed.

### Example:
After the call to this function we have
`k=new_indexes_of_fens1_nodes[j]` is the node in the node set fens which
used to be node `j` in node set `fens1`.
The finite element set connectivity that used to refer to `fens1`
needs to be updated to refer to the same nodes in  the set `fens` as
     `updateconn!(fes, new_indexes_of_fens1_nodes);`
"""
function fusenodes(fens1::FENodeSet, fens2::FENodeSet, tolerance:: FFlt)
    @assert size(fens1.xyz, 2) == size(fens2.xyz, 2)
    dim::FInt = size(fens1.xyz,2);
    nn1::FInt = count(fens1)
    nn2::FInt = count(fens2)
    xyz1 = zeros(FFlt,nn1,dim); copyto!(xyz1, fens1.xyz)#::FFltMat = copy(fens1.xyz::FFltMat)
    id1 = collect(1:nn1);
    xyz2 = zeros(FFlt,nn2,dim); copyto!(xyz2, fens2.xyz)#xyz2::FFltMat = copy(fens2.xyz::FFltMat)
    id2 = collect(1:nn2);
    # Decide which nodes should be checked for proximity
    ib::FFltVec = intersectboxes(inflatebox!(boundingbox(xyz1), tolerance), inflatebox!(boundingbox(xyz2), tolerance))
    node1in = fill(false, nn1);
    node2in = fill(false, nn2);
    if length(ib) > 0
        for i=1:nn1
            node1in[i] = inbox(ib, @view xyz1[i, :])
        end
        for i=1:nn2
            node2in[i] = inbox(ib, @view xyz2[i, :])
        end
    end
    # Mark nodes from the first array that are duplicated in the second
    if (tolerance > 0.0) # should we attempt to merge nodes?
        for i=1:nn1
            if node1in[i]
                breakoff = false
                for rx=1:nn2
                    if node2in[rx]
                        distance::FFlt= 0.0
                        for cx=1:dim
                            distance = distance + abs(xyz2[rx,cx]-xyz1[i,cx]);
                            if (distance >= tolerance) # shortcut: if the distance is already too large, stop checking
                                break
                            end
                        end
                        if (distance < tolerance)
                            id1[i] = -rx; breakoff = true;
                        end
                    end
                    if breakoff
                        break
                    end
                end
            end
        end
    end
    # Generate  fused arrays of the nodes. First copy in the nodes from the second set...
    xyzm = zeros(FFlt,nn1+nn2,dim);
    for rx = 1:nn2
        for cx = 1:dim
            xyzm[rx,cx] = xyz2[rx,cx];
        end
    end
    idm = zeros(FInt,nn1+nn2);
    for rx = 1:nn2
        idm[rx] = rx;
    end
    mid=nn2+1;
    # ...and then we add in only non-duplicated nodes from the first set
    for i=1:nn1
        if id1[i]>0
            id1[i] = mid;
            idm[mid] = mid;
            for cx = 1:dim
                xyzm[mid,cx] = xyz1[i,cx];
            end
            mid = mid+1;
        else
            id1[i] = id2[-id1[i]];
        end
    end
    nnodes = mid-1;
    xyzm = xyzm[1:nnodes,:];

    # Create the fused Node set
    fens = FENodeSet(xyzm);
    # The Node set 1 numbering will change
    new_indexes_of_fens1_nodes = id1[:];
    # The node set 2 numbering stays the same
    return fens, new_indexes_of_fens1_nodes
end

"""
    compactnodes(fens::FENodeSet, connected::Vector{Bool})

Compact the finite element node set by deleting unconnected nodes.

`fens` = array of finite element nodes
`connected` = The array element `connected[j]` is either 0 (when `j` is an
  unconnected node), or a positive number (when node `j` is connected to
  other nodes by at least one finite element)

### Output:
`fens` = new set of finite element nodes
`new_numbering`= array which tells where in the new `fens` array the
     connected nodes are (or 0 when the node was unconnected). For instance,
     node 5 was connected, and in the new array it is the third node: then
     `new_numbering[5]` is 3.

### Examples:
Let us say there are nodes not connected to any finite element that you
would like to remove from the mesh: here is how that would be
accomplished.
```
connected = findunconnnodes(fens, fes);
fens, new_numbering = compactnodes(fens, connected);
fes = renumberconn!(fes, new_numbering);
```
Finally, check that the mesh is valid:
```
validate_mesh(fens, fes);
```
"""
function compactnodes(fens::FENodeSet, connected::BitArray{1})
    @assert length(connected) == count(fens)
    new_numbering = zeros(FInt,count(fens),1);
    nxyz = deepcopy(fens.xyz);
    id=1;
    for i=1:length(connected)
        if (connected[i])
            new_numbering[i] = id;
            nxyz[id,:] = fens.xyz[i,:];
            id=id+1;
        end
    end
    #new_numbering = new_numbering[1:id-1];
    fens = FENodeSet(nxyz[1:id-1,:]);
    return fens, vec(new_numbering)
end

"""
    mergemeshes(fens1::FENodeSet, fes1::T1,
      fens2::FENodeSet, fes2::T2, tolerance::FFlt) where {T1<:FESet,T2<:FESet}

Merge together two meshes.

Merge two meshes together by gluing together nodes within tolerance. The
two meshes, `fens1`, `fes1`, and `fens2`, `fes2`, are glued together by merging
the nodes that fall within a box of size `tolerance`. If `tolerance` is set
to zero, no merging of nodes is performed; the two meshes are simply
concatenated together.

The merged node set, `fens`, and the two finite element sets with
renumbered  connectivities are returned.

Important notes: On entry into this function the connectivity of `fes1`
point into `fens1` and the connectivity of `fes2` point into `fens2`. After
this function returns the connectivity of both `fes1` and `fes2` point into
`fens`. The order of the nodes of the node set `fens1` in the resulting set
`fens` will have changed, whereas the order of the nodes of the node set
`fens2` is are guaranteed to be the same. Therefore, the connectivity of
`fes2` will in fact remain the same.
"""
function mergemeshes(fens1::FENodeSet, fes1::T1,
    fens2::FENodeSet, fes2::T2, tolerance::FFlt) where {T1<:FESet,T2<:FESet}
    # Fuse the nodes
    # @code_warntype fusenodes(fens1, fens2, tolerance);
    fens, new_indexes_of_fens1_nodes = fusenodes(fens1, fens2, tolerance);
    # Renumber the finite elements
    newfes1 = deepcopy(fes1)
    updateconn!(newfes1, new_indexes_of_fens1_nodes);
    # Note that now the connectivity of both fes1 and fes2 point into
    # fens.
    return fens, newfes1, fes2
end

"""
    mergenmeshes(meshes::Array{Tuple{FENodeSet, FESet}}, tolerance::FFlt)

Merge several meshes together.

The meshes are glued together by merging the nodes that fall within
a box of size `tolerance`. If `tolerance` is set to zero, no merging of
nodes is performed; the nodes from the meshes are simply concatenated together.

## Output
The merged node set, `fens`, and an array of finite element sets with
renumbered  connectivities are returned.
"""
function mergenmeshes(meshes::Array{Tuple{FENodeSet, FESet}}, tolerance::FFlt)
    outputfes = Array{FESet,1}()
    if (length(meshes)) == 1 # A single mesh, package output and return
        fens, fes = meshes[1];
        push!(outputfes, fes)
        return fens, outputfes
    end
    # Multiple meshes: process
    fens, fes = meshes[1];
    push!(outputfes, fes)
    for j=2:length(meshes)
        fens1, fes1 = meshes[j];
        fens, new_indexes_of_fens1_nodes = fusenodes(fens1, fens, tolerance);
        updateconn!(fes1,new_indexes_of_fens1_nodes);
        push!(outputfes, fes1)
    end
    return fens, outputfes
end

"""
    mergenodes(fens::FENodeSet, fes::FESet, tolerance::FFlt)

Merge together  nodes of a single node set.

Merge by gluing together nodes from a single node set located within
tolerance of each other. The nodes are glued together by merging the
nodes that fall within a box of size `tolerance`. The merged node
set, `fens`, and the finite element set, `fes`, with renumbered  connectivities
are returned.

Warning: This tends to be an expensive operation!
"""
function mergenodes(fens::FENodeSet, fes::FESet, tolerance::FFlt)
    maxnn = count(fens) + 1
    xyz1 = fens.xyz;
    dim  = size(xyz1,2);
    id1 = collect(1:count(fens));
    d = zeros(size(xyz1,1));
    # Mark nodes from the array that are duplicated
    for i = 1:count(fens)
        if (id1[i] > 0) # This node has not yet been marked for merging
            XYZ = reshape(xyz1[i,:], 1, dim);
            copyto!(d, sum(abs.(xyz1 .- XYZ), dims = 2)); #find the distances along  coordinate directions
            minn = maxnn
            @inbounds for jx = 1:length(d)
                if d[jx] < tolerance
                    minn = min(jx, minn);
                    id1[jx] = -minn;
                    id1[minn] = minn;
                end
            end
        end
    end
    # Generate  merged arrays of the nodes
    xyzm = zeros(FFlt,count(fens),dim);
    mid = 1;
    for i = 1:count(fens) # and then we pick only non-duplicated fens1
        if id1[i] > 0 # this node is the master
            id1[i] = mid;
            xyzm[mid,:] = xyz1[i,:];
            mid = mid+1;
        else # this node is the slave
            id1[i] = id1[-id1[i]];
        end
    end
    nnodes = mid-1;
    xyzm = xyzm[1:nnodes,:];
    # Renumber the cells
    conns = connasarray(fes);
    for i = 1:size(conns,1)
        conns[i,:] = id1[conns[i,:]];
    end
    fes = fromarray!(fes, conns)

    fens = FENodeSet(xyzm[1:nnodes,:]);

    return fens,fes
end

"""
    mergenodes(fens::FENodeSet, fes::FESet, tolerance::FFlt, candidates::FIntVec)

Merge together  nodes of a single node set.

Similar to `mergenodes(fens::FENodeSet, fes::FESet, tolerance::FFlt)`, but only
the candidate nodes are considered for merging. This can potentially speed up the operation by orders of magnitude.
"""
function mergenodes(fens::FENodeSet, fes::FESet, tolerance::FFlt, candidates::FIntVec)
    maxnn = count(fens) + 1
    xyz1 = fens.xyz;
    dim  = size(xyz1,2);
    id1 = collect(1:count(fens));
    d = fill(100.0*tolerance, size(xyz1, 1))
    # Mark nodes from the array that are duplicated
    for ic = 1:length(candidates)
        i = candidates[ic]
        if (id1[i] > 0) # This node has not yet been marked for merging
            XYZ = xyz1[i,:];
            minn = maxnn
            for kx = candidates
                d[kx] = sum(abs.(xyz1[kx,:] .- XYZ))
            end
            @inbounds for jx = candidates
                if d[jx] < tolerance
                    minn = min(jx, minn);
                    id1[jx] = -minn;
                    id1[minn] = minn;
                end
            end
        end
    end
    # Generate  merged arrays of the nodes
    xyzm = zeros(FFlt,count(fens),dim);
    mid = 1;
    for i = 1:count(fens) # and then we pick only non-duplicated fens1
        if id1[i] > 0 # this node is the master
            id1[i] = mid;
            xyzm[mid,:] = xyz1[i,:];
            mid = mid+1;
        else # this node is the slave
            id1[i] = id1[-id1[i]];
        end
    end
    nnodes = mid-1;
    xyzm = xyzm[1:nnodes,:];
    # Renumber the cells
    conns = connasarray(fes);
    for i = 1:count(fes)
        conn = conns[i,:];
        conns[i,:] = id1[conn];
    end
    fes = fromarray!(fes, conns);

    fens = FENodeSet(xyzm[1:nnodes,:]);

    return fens,fes
end

"""
    renumberconn!(fes::FESet, new_numbering::FIntVec)

Renumber the nodes in the connectivity of the finite elements based on a new
numbering for the nodes.

`fes` =finite element set
`new_numbering` = new serial numbers for the nodes.  The connectivity
          should be changed as `conn[j]` --> `new_numbering[conn[j]]`

Let us say there are nodes not connected to any finite element that you would
like to remove from the mesh: here is how that would be accomplished.
```
connected = findunconnnodes(fens, fes);
fens, new_numbering = compactfens(fens, connected);
fes = renumberconn!(fes, new_numbering);
```
Finally, check that the mesh is valid:
```julia
validate_mesh(fens, fes);
```
"""
function renumberconn!(fes::FESet, new_numbering::FIntVec)
    conn = connasarray(fes)
    for i=1:size(conn,1)
        c = conn[i,:];
        conn[i,:] = new_numbering[c];
    end
    return fromarray!(fes, conn)
end

"""
    vsmoothing(v::FFltMat, t::FIntMat; options...)

Internal routine for mesh smoothing.

Keyword options:
`method` = :taubin (default) or :laplace
`fixedv` = Boolean array, one entry per vertex: is the vertex immovable (true)
    or movable  (false)
`npass` = number of passes (default 2)
"""
function vsmoothing(v::FFltMat, t::FIntMat; kwargs...)
    fixedv = falses(size(v,1))
    npass = 2;
    method =:taubin;
    for apair in pairs(kwargs)
        sy, val = apair
        if sy==:method
            method = val
        elseif sy==:fixedv
            fixedv .= val
        elseif sy==:npass
            npass = val
        end
    end

    nv = deepcopy(v)
    # find neighbors for the given connections
    vneigh =  vertexneighbors(t,size(v,1));
    # Smoothing considering all connections through the volume
    if (method == :taubin)
        nv =  smoothertaubin(v,vneigh,fixedv,npass,0.5,-0.5);
    elseif (method == :laplace)
        nv =  smootherlaplace(v,vneigh,fixedv,npass,0.5,-0.5);
    end
    # return new vertex locations
    return nv
end

"""
    meshsmoothing(fens::FENodeSet, fes::T; options...) where {T<:FESet}

General smoothing of meshes.

## Keyword options:
`method` = :taubin (default) or :laplace
`fixedv` = Boolean array, one entry per vertex: is the vertex immovable (true)
    or movable  (false)
`npass` = number of passes (default 2)

## Return
The modified  node set.
"""
function meshsmoothing(fens::FENodeSet, fes::T; options...) where {T<:FESet}
    v = deepcopy(fens.xyz)
    v = vsmoothing(v, connasarray(fes); options...)
    copyto!(fens.xyz, v)
    return fens
end

function  smoothertaubin(vinp::FFltMat, vneigh::Array{FIntVec,1}, fixedv::T, npass::FInt, lambda::FFlt, mu::FFlt) where {T}
    v=deepcopy(vinp);
    nv=deepcopy(vinp);
    for I= 1:npass
        o=randperm(length(vneigh));
        damping_factor=lambda;
        for k= 1:length(vneigh)
            r=o[k];
            n=vneigh[r];
            if (length(n)>1) && (!fixedv[r])
                ln1 = (length(n)-1)
                nv[r,:] .= (1-damping_factor)*vec(v[r,:]) + damping_factor*(vec(sum(v[n,:], dims = 1)) - vec(v[r,:]))/ln1;
            end
        end
        v=deepcopy(nv);
        damping_factor=mu;
        for k= 1:length(vneigh)
            r=o[k];
            n=vneigh[r];
            if (length(n)>1) && (!fixedv[r])
                ln1 = (length(n)-1)
                nv[r,:] .= (1-damping_factor)*vec(v[r,:]) + damping_factor*(vec(sum(v[n,:], dims = 1)) - vec(v[r,:]))/ln1;
            end
        end
        v=deepcopy(nv);
    end
    return nv
end

function   smootherlaplace(vinp::FFltMat, vneigh::Array{FIntVec,1}, fixedv::T, npass::FInt, lambda::FFlt,mu::FFlt) where {T}
    v=deepcopy(vinp);
    nv=deepcopy(vinp);
    damping_factor=lambda;
    for I= 1:npass
        o=randperm(length(vneigh));
        for k= 1:length(vneigh)
            r=o[k];
            n=vneigh[r];
            if (length(n)>1) && (!fixedv[r])
                ln1 = (length(n)-1)
                nv[r,:] = (1-damping_factor)*vec(v[r,:]) + damping_factor*(vec(sum(v[n,:], dims = 1))-vec(v[r,:]))/ln1;
            end
        end
        v=deepcopy(nv);
    end
    return nv
end

"""
    vertexneighbors(conn::FIntMat, nvertices::FInt)

Find the node neighbors in the mesh.

Return an array of integer vectors, element I holds an array of numbers of nodes
which are connected to node I (including node I).
"""
function vertexneighbors(conn::FIntMat, nvertices::FInt)
    vn = FIntVec[]; sizehint!(vn, nvertices)
    for I= 1:nvertices
        push!(vn, FInt[]);          # preallocate
    end
    for I= 1:size(conn,1)
        for r= 1:size(conn,2)
            append!(vn[conn[I,r]],vec(conn[I,:]));
        end
    end
    for I= 1:length(vn)
        vn[I]=unique(vn[I]);
    end
    return vn
end

"""
    mirrormesh(fens::FENodeSet, fes::T, Normal::FFltVec,
      Point::FFltVec; kwargs...) where {T<:FESet}

Mirror a 2-D mesh in a plane given by its normal and one point.

Warning: The code to relies on the numbering of the cells: to reverse the
orientation of the mirrored cells, the connectivity is listed in reverse
order.   If the mirrored cells do not follow this rule (for instance hexahedra
for quadrilaterals), their areas/volumes will come out negative. In such a
case the renumbering function of the connectivity needs to be supplied.

For instance: H8 elements require  the renumbering function to be supplied as
fens1,gcells1 = mirror_mesh(fens, gcells,...
          [-1,0,0], [0,0,0], @(c)c([1, 4, 3, 2, 5, 8, 7, 6]));
"""
function mirrormesh(fens::FENodeSet, fes::T, Normal::FFltVec,
    Point::FFltVec; kwargs...) where {T<:FESet}
    # Treat optional arguments.
    # Simply switch the order of nodes.  Works for simplexes...
    renumb(conn) = conn[end:-1:1];
    for apair in pairs(kwargs)
        sy, val = apair
        if sy == :renumb
            renumb = val
        end
    end
    # Make sure we're using a unit normal
    Normal = Normal/norm(Normal);
    Normal = vec(Normal)
    # The point needs to be a row  matrix
    Point = vec(Point)

    fens1 = deepcopy(fens); # the mirrored mesh nodes
    for i = 1:count(fens1)
        a = fens1.xyz[i,:]
        d = dot(vec(a-Point), Normal);
        fens1.xyz[i,:] = a-2*d*Normal;
    end
    # Reconnect the cells
    fes1=deepcopy(fes);
    conn = connasarray(fes1)
    for i=1:size(conn, 1)
        conn[i,:]=renumb(conn[i,:]);
    end
    return fens1, fromarray!(fes1, conn)
end

function _nodepartitioning3(fens::FENodeSet, nincluded::Vector{Bool}, npartitions::Int = 2)
    function inertialcutpartitioning!(partitioning, parts, X)
        nspdim = 3
        StaticMoments = fill(zero(FFlt), nspdim, length(parts));
        npart = fill(0, length(parts))
        for spdim = 1:nspdim
            @inbounds for j = 1:size(X, 1)
                if nincluded[j] # Is the node to be included in the partitioning?
                    jp = partitioning[j]
                    StaticMoments[spdim, jp] += X[j, spdim]
                    npart[jp] += 1 # count the nodes in the current partition
                end
            end
        end
        CG = fill(zero(FFlt), nspdim, length(parts));
        for p = parts
            npart[p] = Int(npart[p] / nspdim)
            CG[:, p] = StaticMoments[:, p] / npart[p] # center of gravity of each partition
        end
        MatrixMomentOfInertia = fill(zero(FFlt), nspdim, nspdim, length(parts))
        @inbounds for j = 1:size(X, 1)
            if nincluded[j] # Is the node to be included in the partitioning?
                jp = partitioning[j]
                xj, yj, zj = X[j, 1] - CG[1, jp], X[j, 2] - CG[2, jp], X[j, 3] - CG[3, jp]
                MatrixMomentOfInertia[1, 1, jp] += yj^2 + zj^2
                MatrixMomentOfInertia[2, 2, jp] += xj^2 + zj^2
                MatrixMomentOfInertia[3, 3, jp] += yj^2 + xj^2
                MatrixMomentOfInertia[1, 2, jp] -= xj * yj
                MatrixMomentOfInertia[1, 3, jp] -= xj * zj
                MatrixMomentOfInertia[2, 3, jp] -= yj * zj
            end
        end
        for p = parts
            MatrixMomentOfInertia[2, 1, p] = MatrixMomentOfInertia[1, 2, p]
            MatrixMomentOfInertia[3, 1, p] = MatrixMomentOfInertia[3, 1, p]
            MatrixMomentOfInertia[3, 2, p] = MatrixMomentOfInertia[3, 2, p]
        end
        longdir = fill(zero(FFlt), nspdim, length(parts))
        for p = parts
            F = eigen(MatrixMomentOfInertia[:, :, p])
            six = sortperm(F.values)
            longdir[:, p] = F.vectors[:, six[1]]
        end
        toggle = fill(one(FFlt), length(parts));
        @inbounds for j = 1:size(X, 1)
            if nincluded[j] # Is the node to be included in the partitioning?
                jp = partitioning[j]
                vx, vy, vz = longdir[:, jp]
                xj, yj, zj = X[j, 1] - CG[1, jp], X[j, 2] - CG[2, jp], X[j, 3] - CG[3, jp]
                d = xj * vx + yj * vy + zj * vz
                c = 0
                if d < 0.0
                    c = 1
                elseif d > 0.0
                    c = 0
                else # disambiguate d[ixxxx] == 0.0
                    c = (toggle[jp] > 0) ? 1 : 0
                    toggle[jp] = -toggle[jp]
                end
                partitioning[j] = 2 * jp - c
            end
        end
    end

    nlevels = Int(round(ceil(log(npartitions)/log(2))))
    partitioning = fill(1, count(fens))  # start with nodes assigned to partition 1
    for level = 0:1:(nlevels - 1)
        inertialcutpartitioning!(partitioning, collect(1:2^level), fens.xyz)
    end
    return partitioning
end

function _nodepartitioning2(fens::FENodeSet, nincluded::Vector{Bool}, npartitions::Int = 2)
    function inertialcutpartitioning!(partitions, parts, X)
        nspdim = 2
        StaticMoments = fill(zero(FFlt), nspdim, length(parts));
        npart = fill(0, length(parts))
        for spdim = 1:nspdim
            @inbounds for j = 1:size(X, 1)
                if nincluded[j] # Is the node to be included in the partitioning?
                    jp = partitions[j]
                    StaticMoments[spdim, jp] += X[j, spdim]
                    npart[jp] += 1 # count the nodes in the current partition
                end
            end
        end
        CG = fill(zero(FFlt), nspdim, length(parts));
        for p = parts
            npart[p] = Int(npart[p] / nspdim)
            CG[:, p] = StaticMoments[:, p] / npart[p] # center of gravity of each partition
        end
        MatrixMomentOfInertia = fill(zero(FFlt), nspdim, nspdim, length(parts))
        @inbounds for j = 1:size(X, 1)
            if nincluded[j] # Is the node to be included in the partitioning?
                jp = partitions[j]
                xj, yj = X[j, 1] - CG[1, jp], X[j, 2] - CG[2, jp]
                MatrixMomentOfInertia[1, 1, jp] += yj^2
                MatrixMomentOfInertia[2, 2, jp] += xj^2
                MatrixMomentOfInertia[1, 2, jp] -= xj * yj
            end
        end
        for p = parts
            MatrixMomentOfInertia[2, 1, p] = MatrixMomentOfInertia[1, 2, p]
        end
        longdir = fill(zero(FFlt), nspdim, length(parts))
        for p = parts
            F = eigen(MatrixMomentOfInertia[:, :, p])
            six = sortperm(F.values)
            longdir[:, p] = F.vectors[:, six[1]]
        end
        toggle = fill(one(FFlt), length(parts));
        @inbounds for j = 1:size(X, 1)
            if nincluded[j] # Is the node to be included in the partitioning?
                jp = partitions[j]
                vx, vy = longdir[:, jp]
                xj, yj = X[j, 1] - CG[1, jp], X[j, 2] - CG[2, jp]
                d = xj * vx + yj * vy
                c = 0
                if d < 0.0
                    c = 1
                elseif d > 0.0
                    c = 0
                else # disambiguate d[ixxxx] == 0.0
                    c = (toggle[jp] > 0) ? 1 : 0
                    toggle[jp] = -toggle[jp]
                end
                partitions[j] = 2 * jp - c
            end
        end
    end

    nlevels = Int(round(ceil(log(npartitions)/log(2))))
    partitions = fill(1, count(fens))  # start with nodes assigned to partition 1
    for level = 0:1:(nlevels - 1)
        inertialcutpartitioning!(partitions, collect(1:2^level), fens.xyz)
    end
    return partitions
end

"""
    nodepartitioning(fens::FENodeSet, nincluded::Vector{Bool}, npartitions)

Compute the inertial-cut partitioning of the nodes.

`nincluded` = Boolean array: is the node to be included in the partitioning or
    not?
`npartitions` = number of partitions, but note that the actual number of
    partitions is going to be a power of two.

The partitioning can be visualized for instance as:
```julia
partitioning = nodepartitioning(fens, npartitions)
partitionnumbers = unique(partitioning)
for gp = partitionnumbers
  groupnodes = findall(k -> k == gp, partitioning)
  File =  "partition-nodes-Dollar(gp).vtk"
  vtkexportmesh(File, fens, FESetP1(reshape(groupnodes, length(groupnodes), 1)))
end
File =  "partition-mesh.vtk"
vtkexportmesh(File, fens, fes)
@async run(`"paraview.exe" DollarFile`)
```
"""
function nodepartitioning(fens::FENodeSet, nincluded::Vector{Bool}, npartitions::Int = 2)
    @assert npartitions >= 2
    if size(fens.xyz, 2) == 3
        return _nodepartitioning3(fens, nincluded, npartitions)
    elseif size(fens.xyz, 2) == 2
        return _nodepartitioning2(fens, nincluded, npartitions)
    else
        @warn "Not implemented for 1D"
    end
end

"""
    nodepartitioning(fens::FENodeSet, npartitions)

Compute the inertial-cut partitioning of the nodes.

`npartitions` = number of partitions, but note that the actual number of
partitions is going to be a power of two.

In this variant all the nodes are to be included in the partitioning.

The partitioning can be visualized for instance as:
```julia
partitioning = nodepartitioning(fens, npartitions)
partitionnumbers = unique(partitioning)
for gp = partitionnumbers
  groupnodes = findall(k -> k == gp, partitioning)
  File =  "partition-nodes-Dollar(gp).vtk"
  vtkexportmesh(File, fens, FESetP1(reshape(groupnodes, length(groupnodes), 1)))
end
File =  "partition-mesh.vtk"
vtkexportmesh(File, fens, fes)
@async run(`"paraview.exe" DollarFile`)
```
"""
function nodepartitioning(fens::FENodeSet, npartitions::Int = 2)
    @assert npartitions >= 2
    nincluded = fill(true, count(fens)) # The default is all nodes are included in the partitioning.
    if size(fens.xyz, 2) == 3
        return _nodepartitioning3(fens, nincluded, npartitions)
    elseif size(fens.xyz, 2) == 2
        return _nodepartitioning2(fens, nincluded, npartitions)
    else
        @warn "Not implemented for 1D"
    end
end

"""
    nodepartitioning(fens::FENodeSet, fesarr, npartitions::Vector{Int})

Compute the inertial-cut partitioning of the nodes.

`fesarr` = array of finite element sets that represent regions
`npartitions` = array of the number of partitions in each region. However
note that the actual number of partitions is going to be a power of two.

The partitioning itself is carried out by `nodepartitioning()` with
a list of nodes to be included in the partitioning. For each region I
the nodes included in the partitioning are those connected to
the elements of that region, but not to elements that belong to
any of the previous regions, 1…I-1.
"""
function nodepartitioning(fens::FENodeSet, fesarr, npartitions::Vector{Int})
    @assert length(fesarr) == length(npartitions)
    # Partitioning of all the nodes
    partitioning = fill(0, count(fens))
    # Find the partitioning of the nodes in FESet 1
    nincludedp = fill(false, count(fens))
    for i = connectednodes(fesarr[1]) # For nodes connected by region 1
        nincludedp[i] = true
    end
    partitioning1 = nodepartitioning(fens, nincludedp, npartitions[1])
    totnpartitions = maximum(partitioning1)
    # Transfer the partitioning of region 1 into the overall partitioning
    partitioning[nincludedp] = partitioning1[nincludedp]
    for i = 2:length(npartitions)
        # Find the partitioning of the nodes in FESet i, but not in the preceding sets
        nincluded = fill(false, count(fens))
        for j = connectednodes(fesarr[i]) # For nodes connected by region i
            nincluded[j] = !nincludedp[j] # Not included previously
        end
        partitioning1 = nodepartitioning(fens, nincluded, npartitions[i])
        np = maximum(partitioning1) # Number of partitions for region i
        partitioning1 = partitioning1 .+ totnpartitions # shift by the number of partitions in previous regions
        totnpartitions = totnpartitions + np # Increment the number of partitions
        partitioning[nincluded] = partitioning1[nincluded] # Update overall partitioning
        @. nincludedp = nincluded | nincludedp  # Update the list of nodes that have already been included
    end
    return partitioning
end

"""
    adjgraph(conn, nfens)

Compute the adjacency graph from the array of connectivities of the elements
in the mesh.

# Examples

```
conn = [9 1 8 4;
       1 3 2 8;
       8 2 7 5;
       2 6 7 7];
nfens = 9;
adjgraph(conn, nfens)

```
should produce
```
9-element Array{Array{Int64,1},1}:
 [9, 8, 4, 3, 2]
 [1, 3, 8, 7, 5, 6]
 [1, 2, 8]
 [9, 1, 8]
 [8, 2, 7]
 [2, 7]
 [8, 2, 5, 6]
 [9, 1, 4, 3, 2, 7, 5]
 [1, 8, 4]
 ```
"""
function adjgraph(conn, nfens)
    neighbors = fill(Int[], nfens)
    for i = 1:length(neighbors)
    	neighbors[i] = Int[]
    end
    for k = 1:size(conn, 1)
    	for node1 = conn[k, :]
    		for node2 = conn[k, :]
    			if node1 != node2
    				push!(neighbors[node1], node2)
    			end
    		end
        end
    end
    for i = 1:length(neighbors)
    	neighbors[i] = unique(neighbors[i])
    end
    return neighbors
end

"""
    _coldeg(A::SparseMatrixCSC, j::Int)

This function has been modified from source of
(https://github.com/rleegates/CuthillMcKee.jl/blob/master/src/CuthillMcKee.jl)

`A` is assumed to be symmetric.
"""
function _coldeg(A::SparseMatrixCSC, j::Int)
	cptr = A.colptr
	return cptr[j+1]-cptr[j]
end

"""
    adjgraph(A)

Compute the adjacency graph from a sparse matrix. This function has been
modified from source of
(https://github.com/rleegates/CuthillMcKee.jl/blob/master/src/CuthillMcKee.jl)

`A` is assumed to be symmetric.
"""
function adjgraph(A::SparseMatrixCSC)
	cptr = A.colptr
	rval = A.rowval
	ncols = length(cptr)-1
	neighbors = Vector{Vector{Int}}(undef, ncols)
	for i = 1:length(neighbors)
		neighbors[i] = Int[]
	end
	sbyf = let A = A
		j->_coldeg(A, j)
	end
	for j = 1:ncols
		strt = cptr[j]
		jdeg = _coldeg(A, j)
		jadj = Vector{Int}(undef, jdeg)
		for i = 1:jdeg
			jadj[i] = rval[strt+i-1]
		end
		sort!(jadj, by=sbyf)
		neighbors[j] = jadj
	end
	return neighbors
end

"""
    nodedegrees(adjgr::Vector{Vector{Int}})

Compute the degrees of the nodes in the adjacency graph.

conn = [9 1 8 4;
       1 3 2 8;
       8 2 7 5;
       2 6 7 7];
nfens = 9;
adjgr = adjgraph(conn, nfens)
nodedegrees(adjgr)

julia> degrees = node_degrees(adjgr)
9-element Array{Int64,1}:
 5
 6
 3
 3
 3
 2
 4
 7
 3
"""
function nodedegrees(adjgr::Vector{Vector{Int}})
    degrees = fill(0, length(adjgr))
    for k = 1:length(adjgr)
        degrees[k] = length(adjgr[k])
    end
    return degrees
end

function _findP(inR, degrees)
	# println("In _findP")
	P = 0
	mindegree = length(inR)
	@inbounds for i = 1:length(inR)
		if !inR[i]
			return i
			# if degrees[i] < mindegree
			# 	P = i
			# 	mindegree = degrees[i]
			# end
		end
	end
	return P
end

"""
    revcm(adjgr::Vector{Vector{Int}}, degrees::Vector{Int})

Reverse Cuthill-McKee node-renumbering algorithm.
"""
function revcm(adjgr::Vector{Vector{Int}}, degrees::Vector{Int})
	@assert length(adjgr) == length(degrees)
	# Initialization
	n = length(adjgr)
	ndegperm = sortperm(degrees) # sorted nodal degrees
	inR = fill(false, n) # Is a node in the result list?
	R = Int64[]
	sizehint!(R, n)
	Q = Int64[] # Node queue
	sizehint!(Q, n)
	while true
		P = 0 # Find the next node to start from
		while !isempty(ndegperm)
			i = popfirst!(ndegperm)
			if !inR[i]
				P = i
				break
			end
		end
		if P == 0
			break # That was the last node
		end
		# Now we have a node to start from: put it into the result list
		push!(R, P); inR[P] = true
		empty!(Q) # empty the queue
		append!(Q, adjgr[P]) # put the adjacent nodes into the queue
		while length(Q) >= 1
			C = popfirst!(Q) # child to put into the result list
			if !inR[C]
				push!(R, C); inR[C] = true
			end
			for i in adjgr[C] # at all adjacent nodes into the queue
				if !inR[i]
					push!(Q, i)
				end
			end
		end
    end
    return reverse(R) # reverse the result list
end

end


text:
# This file is used for get angle-of-attack
# be carful of the coordination

function aoaget(vall_r,ψ,θ0,θ_lat,θ_lon,beta=0.0)
#The solution is directly solving the angle between
#pitch angle vector and wind angle vector.

  α_aero = Array{Float64}(NR,Nbe)
  θ = Array{Float64}(NR,Nbe)

  for k in 1:NR
    ψk = ψ+(k-1)*2*π/NR
    for i in 1:Nbe
      v_per = vall_r[k,i][3]
      v_tan = vall_r[k,i][2]
      v_rad = vall_r[k,i][1]
      θ[k,i] = θ0[k,i]+θ_lat[k]*cos(ψk)+θ_lon[k]*sin(ψk) #pitch angle

      wind_v = [0,v_tan,v_per] #wind velocity
      wind_ = wind_v/norm(wind_v) #unit vector of wind velocity
      wind_tmp = rotobe(wind_,beta,θ[k,i])
      wind_be = [wind_tmp[2],wind_tmp[3]] # 叶素当地二维气流速度
      α_aero[k,i] = aoaang(wind_be) # 最终的气动迎角解
    end
  end
  return α_aero,θ #,dF_be
end

ext: jl

max_stars_repo_name: dingzhw/RotorTrim

max_issues_repo_name: helicopterliu/RotorTrim

max_forks_repo_name: helicopterliu/RotorTrim


text:
@reexport module MaximizationBias

export MaximizationBiasEnv

using ReinforcementLearningBase

"""
states:
1:A
2:B
3:terminal

actions:
1: left
2: right
"""
Base.@kwdef mutable struct MaximizationBiasEnv <: AbstractEnv
    position::Int = 1
    reward::Float64 = 0.0
end

RLBase.get_observation_space(env::MaximizationBiasEnv) = DiscreteSpace(3)
RLBase.get_action_space(env::MaximizationBiasEnv) = DiscreteSpace(10)

const LEFT = 1

function (env::MaximizationBiasEnv)(a::Int)
    if env.position == 1
        if a == 1
            env.position = 2
            env.reward = 0.0
        else
            env.position = 3
            env.reward = 0.0
        end
    elseif env.position == 2
        env.position = 3
        env.reward = randn() - 0.1
    end
    nothing
end

function RLBase.reset!(env::MaximizationBiasEnv)
    env.position = 1
    env.reward = 0.0
    nothing
end

RLBase.observe(env::MaximizationBiasEnv) =
    (reward = env.reward, terminal = env.position == 3, state = env.position)

end


text:
#TODO move this to EcoBase

groupsites(a::EcoBase.AbstractAssemblage, s::Symbol) = groupsites(a, a[s])
groupsites(a::EcoBase.AbstractAssemblage, s::AbstractVector) = [view(a, sites = [(!ismissing(y) && y == x) for y in s]) for x in sort(unique(s))]
groupspecies(a::EcoBase.AbstractAssemblage, s::Symbol) = groupspecies(a, a[s])
groupspecies(a::EcoBase.AbstractAssemblage, s::AbstractVector) = [view(a, species = [(!ismissing(y) && y == x) for y in s]) for x in sort(unique(s))]
const GroupedAssemblage = Vector{T} where T<:EcoBase.AbstractAssemblage

function show(io::IO, com::GroupedAssemblage)
    println(io, "GroupedAssemblage with $(length(com)) assemblages")
end


text:
@diffusion_process FitzHughDiffusion begin
    :dimensions
    process --> 2
    wiener --> 1

    :parameters
    (ϵ, s, γ, β, σ) --> Float64

    :additional
    constdiff --> true
end

function b(t, x, P::FitzHughNagumo)
    ℝ{2}(
        (x[1] - x[2] - x[1]^3 + P.s)/P.ϵ,
        P.γ*x[1] - x[2] + P.β
    )
end

σ(t, x, P::FitzHughNagumo) = ℝ{2}(0.0, P.σ)


text:
#SD_HVI functions

export SD_HVI_load, SD_HVI_open, SD_HVI_close, SD_HVI_start, SD_HVI_pause, SD_HVI_resume, SD_HVI_stop, SD_HVI_reset, SD_HVI_compile,
	SD_HVI_compilationErrorMessage, SD_HVI_getNumberOfModules, SD_HVI_getModuleName, SD_HVI_assignHardwareWithIndexAndSlot,
	SD_HVI_assignHardwareWithUserNameAndSerialNumber, SD_HVI_assignHardwareWithUserNameAndSlot,
	SD_HVI_assignHardwareWithUserNameAndModuleID, SD_HVI_getModuleIndex, SD_HVI_getModuleIDwithIndex,
	SD_HVI_getModuleIDwithUserName, SD_HVI_writeIntegerConstantWithIndex, SD_HVI_writeIntegerConstantWithUserName,
	SD_HVI_writeDoubleConstantWithIndex, SD_HVI_writeDoubleConstantWithUserName, SD_HVI_readIntegerConstantWithIndex,
	SD_HVI_readIntegerConstantWithUserName, SD_HVI_readDoubleConstantWithIndex, SD_HVI_readDoubleConstantWithUserName

# Load and run existing HVI

#int SD_HVI_open(char* HVIfile);
SD_HVI_open(HVIfile::String) =
	ccall((:SD_HVI_open, klib), Cint, (Cstring,), HVIfile)
#int SD_HVI_close(int HVIID);
SD_HVI_close(HVIID::Integer) =
	ccall((:SD_HVI_close, klib), Cint, (Cint,), HVIID)
#int SD_HVI_load(int HVIID);
SD_HVI_load(HVIID::Integer) =
	ccall((:SD_HVI_load, klib), Cint, (Cint,), HVIID)
#int SD_HVI_start(int HVIID);
SD_HVI_start(HVIID::Integer) =
	ccall((:SD_HVI_start, klib), Cint, (Cint,), HVIID)
#int SD_HVI_pause(int HVIID);
SD_HVI_pause(HVIID::Integer) =
	ccall((:SD_HVI_pause, klib), Cint, (Cint,), HVIID)
#int SD_HVI_resume(int HVIID);
SD_HVI_resume(HVIID::Integer) =
	ccall((:SD_HVI_resume, klib), Cint, (Cint,), HVIID)
#int SD_HVI_stop(int HVIID);
SD_HVI_stop(HVIID::Integer) =
	ccall((:SD_HVI_stop, klib), Cint, (Cint,), HVIID)
#int SD_HVI_reset(int HVIID);
SD_HVI_reset(HVIID::Integer) =
	ccall((:SD_HVI_reset, klib), Cint, (Cint,), HVIID)
#int SD_HVI_compile(int HVIID);
SD_HVI_compile(HVIID::Integer) =
	ccall((:SD_HVI_compile, klib), Cint, (Cint,), HVIID)
#int SD_HVI_compilationErrorMessage(int HVIID, int errorIndex, char *message, int maxSize);
SD_HVI_compilationErrorMessage(HVIID::Integer, errorIndex::Integer, message::String, maxSize::Integer) =
	ccall((:SD_HVI_compilationErrorMessage, klib), Cint, (Cint, Cint, Cstring, Cint), HVIID, errorIndex, mesasge, maxSize)

# Assign Hardware

#int SD_HVI_assignHardwareWithIndexAndSerialNumber(int HVIID, int index, const char *productName, const char *serialNumber);
SD_HVI_assignHardwareWithIndexAndSerialNumber(HVIID::Integer, index::Integer, productName::String, serialNumber::String) =
ccall((:SD_HVI_assignHardwareWithIndexAndSerialNumber, klib), Cint, (Cint, Cint, Cstring, Cstring), HVIID, index, productName, serialNumber)

#int SD_HVI_assignHardwareWithIndexAndSlot(int HVIID, int index, int chassis, int slot);
SD_HVI_assignHardwareWithIndexAndSlot(HVIID::Integer, index::Integer, chassis::Integer, intslot::Integer) =
	ccall((:SD_HVI_assignHardwareWithIndexAndSlot, klib), Cint, (Cint, Cint, Cint, Cint), HVIID, index, chassis, intslot)

#int SD_HVI_assignHardwareWithUserNameAndSerialNumber(int HVIID, const char *moduleUserName, const char *productName, const char *serialNumber);
SD_HVI_assignHardwareWithUserNameAndSerialNumber(HVIID::Integer, moduleUserName::String, productName::String, serialNumber::String) =
	ccall((:SD_HVI_assignHardwareWithUserNameAndSerialNumber, klib), Cint, (Cint, Cstring, Cstring, Cstring), HVIID, moduleUserName, productName, serialNumber)

#int SD_HVI_assignHardwareWithUserNameAndSlot(int HVIID, const char *moduleUserName, int chassis, int slot);
SD_HVI_assignHardwareWithUserNameAndSlot(HVIID::Integer, moduleUserName::String, chassis::Integer, intslot::Integer) =
	ccall((:SD_HVI_assignHardwareWithUserNameAndSlot, klib), Cint, (Cint, Cstring, Cint, Cint), HVIID, moduleUserName, chassis, intslot)

#int SD_HVI_assignHardwareWithUserNameAndModuleID(int HVIID, const char *moduleUserName, int module);
SD_HVI_assignHardwareWithUserNameAndModuleID(HVIID::Integer, moduleUserName::String, moduleID::Integer) =
	ccall((:SD_HVI_assignHardwareWithUserNameAndModuleID, klib), Cint, (Cint, Cstring, Cint), HVIID, moduleUserName, moduleID)

# Get module info

#int SD_HVI_getNumberOfModules(int HVIID);
SD_HVI_getNumberOfModules(HVIID::Integer) =
	ccall((:SD_HVI_getNumberOfModules, klib), Cint, (Cint,), HVIID)

#int SD_HVI_getModuleName(int HVIID, int index, char *buffer, int size);
SD_HVI_getModuleName(HVIID::Integer, index::Integer, buffer::String, size::Integer) =
	ccall((:SD_HVI_getModuleName, klib), Cint, (Cint, Cint, Cstring, Cint), HVIID, index, buffer, size)

#int SD_HVI_getModuleIndex(int HVIID, const char *moduleUserName);
SD_HVI_getModuleIndex(HVIID::Integer, moduleUserName::String) =
	ccall((:SD_HVI_getModuleIndex, klib), Cint, (Cint, Cstring), HVIID, moduleUserName)

#int SD_HVI_getModuleIDwithIndex(int HVIID, int index);
SD_HVI_getModuleIDwithIndex(HVIID::Integer, index::Integer) =
	ccall((:SD_HVI_getModuleIDwithIndex, klib), Cint, (Cint, Cint), HVIID, index)

#int SD_HVI_getModuleIDwithUserName(int HVIID, const char *moduleUserName);
SD_HVI_getModuleIDwithUserName(HVIID::Integer, moduleUserName::String) =
	ccall((:SD_HVI_getModuleIDwithUserName, klib), Cint, (Cint, Cstring), HVIID, moduleUserName)

# Communicate with HVI's constants

#int SD_HVI_writeIntegerConstantWithIndex(int HVIID, int moduleIndex, const char *constantName, int value);
SD_HVI_writeIntegerConstantWithIndex(HVIID::Integer, moduleIndex::Integer, constantName::String, value::Integer) =
	ccall((:SD_HVI_writeIntegerConstantWithIndex, klib), Cint, (Cint, Cint, Cstring, Cint), HVIID, moduleIndex, constantName, value)

#int SD_HVI_writeIntegerConstantWithUserName(int HVIID, const char *moduleUserName, const char *constantName, int value);
SD_HVI_writeIntegerConstantWithUserName(HVIID::Integer, moduleUserName::String, constantName::String, value::Integer) =
	ccall((:SD_HVI_writeIntegerConstantWithUserName, klib), Cint, (Cint, Cstring, Cstring, Cint), HVIID, moduleUserName, constantName, value)

#int SD_HVI_writeDoubleConstantWithIndex(int HVIID, int moduleIndex, const char *constantName, double value, const char *unit);
SD_HVI_writeDoubleConstantWithIndex(HVIID::Integer, moduleIndex::Integer, constantName::String, value::Float64, unit::String) =
	ccall((:SD_HVI_writeDoubleConstantWithIndex, klib), Cint, (Cint, Cint, Cstring, Cdouble, Cstring), HVIID, moduleIndex, constantName, value, unit)

#int SD_HVI_writeDoubleConstantWithUserName(int HVIID, const char *moduleUserName, const char *constantName, double value, const char *unit);
SD_HVI_writeDoubleConstantWithUserName(HVIID::Integer, moduleUserName::String, constantName::String, value::Float64, unit::String) =
	ccall((:SD_HVI_writeDoubleConstantWithUserName, klib), Cint, (Cint, Cstring, Cstring, Cdouble, Cstring), HVIID, moduleUserName, constantName, value, unit)

#int SD_HVI_readIntegerConstantWithIndex(int HVIID, int moduleIndex, const char *constantName, int &value);
function SD_HVI_readIntegerConstantWithIndex(HVIID::Integer, moduleIndex::Integer, constantName::String)
	value = Ref{Cint}(0)
	ret = ccall((:SD_HVI_readIntegerConstantWithIndex, klib), Cint, (Cint, Cint, Cstring, Ref{Cint}), HVIID, moduleIndex, constantName, value)
	return ret, value
end

#int SD_HVI_readIntegerConstantWithUserName(int HVIID, const char *moduleUserName, const char *constantName, int &value);
function SD_HVI_readIntegerConstantWithUserName(HVIID::Integer, moduleUserName::String, constantName::String)
	value = Ref{Cint}(0)
	ret = ccall((:SD_HVI_readIntegerConstantWithUserName, klib), Cint, (Cint, Cstring, Cstring, Ref{Cint}), HVIID, moduleUserName, constantName, value)
	return ret, value
end

#int SD_HVI_readDoubleConstantWithIndex(int HVIID, int moduleIndex, const char *constantName, double &value, const char * &unit);
function SD_HVI_readDoubleConstantWithIndex(HVIID::Integer, moduleIndex::Integer, constantName::String)
	value = Ref{Cdouble}(0)
	unit = Ref{Cstring}("")
	ret = ccall((:SD_HVI_readDoubleConstantWithIndex, klib), Cint, (Cint, Cint, Cstring, Ref{Cdouble}, Ref{Cstring}), HVIID, moduleIndex, constantName, value, unit)
	return ret, value, unit
end

#int SD_HVI_readDoubleConstantWithUserName(int HVIID, const char *moduleUserName, const char *constantName, double &value, const char * &unit);
function SD_HVI_readDoubleConstantWithUserName(HVIID::Integer, moduleUserName::String, constantName::String)
	value = Ref{Cdouble}(0)
	unit = Ref{Cstring}("")
	ret = ccall((:SD_HVI_readDoubleConstantWithUserName, klib), Cint, (Cint, Cstring, Cstring, Ref{Cdouble}, Ref{Cstring}), HVIID, moduleUserName, constantName, value, unit)
	return ret, value, unit
end


text:
using LinearAlgebra

# Struct containing the information for cmd
struct ExtraMD
    # First player
    # ############################################################
    # The present optimization variable for x
    x̄

    # An initial guess to use when solving for x
    x̄_guess

    # The dual potential's derivative that is used to map a dual variable to its primal
    Dxψ

    # The dual potential's Hessian
    Dxxψ

    # The inverse of the dual potential's Hessian, which is used precondition the conjugate gradient solver
    Dxxψ_inv

    # Gradient of x's loss function
    ∇_xf

    # Second player
    # ############################################################
    # The present optimization variable for y
    ȳ

    # An initial guess to use when solving for y
    ȳ_guess

    # The dual potential's derivative that is used to map a dual variable to its primal
    Dyψ

    # The dual potential's Hessian
    Dyyψ

    # The inverse of the dual potential's Hessian, which is used precondition the conjugate gradient solver
    Dyyψ_inv

    # Gradient of ys loss function
    ∇_yg
end

function ExtraMD(x, xPotential, f, y, yPotential, g)
  return ExtraMD(x, zero(x), xPotential[1], xPotential[3], xPotential[4], ∇_x(f), y, zero(y), yPotential[1], yPotential[3], yPotential[4], ∇_y(g))
end

function step!(extramd::ExtraMD)
  x = extramd.Dxψ(extramd.x̄)
  Dxxψ = extramd.Dxxψ(x)
  Dxxψ_inv = extramd.Dxxψ_inv(x)

  y = extramd.Dyψ(extramd.ȳ)
  Dyyψ = extramd.Dyyψ(y)
  Dyyψ_inv = extramd.Dyyψ_inv(y)

  ∇_xf = extramd.∇_xf(x, y)
  ∇_yg = extramd.∇_yg(x, y)

  extramd.x̄_guess .= - Dxxψ_inv * (Dxxψ * ∇_xf)
  extramd.ȳ_guess .= - Dyyψ_inv * (Dyyψ * ∇_yg)

  # Computing the position of the extra step
  extra_x = extramd.Dxψ(extramd.x̄ + extramd.x̄_guess)
  extra_y = extramd.Dxψ(extramd.ȳ + extramd.ȳ_guess)

  # Computing the gradient in the extra step
  ∇_xf = extramd.∇_xf(extra_x, extra_y)
  ∇_yg = extramd.∇_yg(extra_x, extra_y)

  extramd.x̄_guess .= - Dxxψ_inv * (Dxxψ * ∇_xf)
  extramd.ȳ_guess .= - Dyyψ_inv * (Dyyψ * ∇_yg)

  extramd.x̄ .+= extramd.x̄_guess
  extramd.ȳ .+= extramd.ȳ_guess

  return 4
end

function iterate_primal(extramd::ExtraMD)
  return extramd.Dxψ(extramd.x̄), extramd.Dyψ(extramd.ȳ)
end

function iterate_dual(extramd::ExtraMD)
  return copy(extramd.x̄), copy(extramd.ȳ)
end

max_stars_repo_name: f-t-s/CMD

max_issues_repo_name: f-t-s/CMD

max_forks_repo_name: f-t-s/CMD


text:
using Test, YaoArrayRegister
using LinearAlgebra

@testset "test fidelity" begin
    reg = rand_state(3)
    reg_ = rand_state(3)
    reg2 = repeat(reg, 3)
    @test fidelity(reg, reg) ≈ 1
    @test fidelity(reg, reg_) < 1
    @test fidelity(reg2, reg2) ≈ [1, 1, 1]

    # mix
    reg4 = join(reg, reg)
    reg5 = join(reg_, reg_)
    focus!(reg4, 1:3)
    focus!(reg5, 1:3)
    @test isapprox(fidelity(reg, reg_), fidelity(reg4, reg5), atol = 1e-5)

    @test isapprox.(
        fidelity(reg, reg_),
        fidelity(repeat(reg4, 3), repeat(reg5, 3)),
        atol = 1e-5,
    ) |> all

    # batch
    st = rand(ComplexF64, 8, 2)
    reg1 = BatchedArrayReg(st)
    reg2 = rand_state(3)

    @test fidelity(reg1, reg2) ≈
          [fidelity(ArrayReg(st[:, 1]), reg2), fidelity(ArrayReg(st[:, 2]), reg2)]

    @test isapprox.(
        fidelity(reg, reg_),
        fidelity(repeat(reg4, 3), repeat(reg5, 3)),
        atol = 1e-5,
    ) |> all
end

@testset "test trace distance" begin
    reg = rand_state(3)
    reg_ = rand_state(3)
    reg2 = repeat(reg, 3)
    dm = ρ(reg)
    dm_ = ρ(reg_)
    dm2s = ρ.(reg2)
    @test reg |> probs ≈ dm |> probs
    @test isapprox(tracedist(dm, dm), tracedist(reg, reg), atol = 1e-5)
    @test isapprox(tracedist(dm, dm_), tracedist(reg, reg_), atol = 1e-5)
    @test isapprox(tracedist.(dm2s, dm2s), tracedist(reg2, reg2), atol = 1e-5)

    # mix
    reg4 = join(reg, reg)
    reg5 = join(reg_, reg_)
    focus!(reg4, 1:3)
    focus!(reg5, 1:3)
    dm4 = reg4 |> density_matrix
    dm5 = reg5 |> density_matrix
    @test nqubits(dm4) == 3
    @test isapprox(tracedist(dm, dm_)[], tracedist(dm4, dm5)[], atol = 1e-5)
    @test isapprox.(
        tracedist(dm, dm_)[],
        tracedist.(repeat(reg4, 3) .|> density_matrix, repeat(reg5, 3) .|> density_matrix),
        atol = 1e-5,
    ) |> all
end

@testset "purify" begin
    reg = rand_state(6)
    reg_p = purify(reg |> ρ)
    @test reg_p |> isnormalized
    @test reg_p |> exchange_sysenv |> probs |> maximum ≈ 1
    reg_p = purify(reg |> ρ; num_env = 0)
    @test fidelity(reg, reg_p) ≈ 1

    reg = rand_state(6; nbatch = 10)
    reg_p = BatchedArrayReg(purify.(reg .|> ρ)...)
    @test reg_p |> isnormalized
    @test reg_p |> exchange_sysenv |> probs |> maximum ≈ 1
    reg_p = BatchedArrayReg(purify.(reg .|> ρ; num_env = 0)...)
    @test fidelity(reg, reg_p) ≈ ones(10)
    reg_p = BatchedArrayReg(purify.(reg .|> ρ; num_env = 2)...)
    @test reg_p |> nqubits == 8
end

@testset "reduce density matrix" begin
    reg = (product_state(bit"00000") + product_state(bit"11111")) / sqrt(2)
    rdm = density_matrix(reg, (1,))
    @test Matrix(rdm) ≈ [1/2 0; 0 1/2]
    reg = product_state([1, 0, 0])
    rdm = density_matrix(reg, (1, 2))
    @test Matrix(rdm) ≈ [0 0 0 0; 0 1 0 0; 0 0 0 0; 0 0 0 0]
end

@testset "von_neumann_entropy" begin
    reg = (product_state(bit"00000") + product_state(bit"11111")) / sqrt(2)
    rho = density_matrix(reg)
    p = eigvals(statevec(reg) * statevec(reg)')
    p = max.(p, eps(Float64))
    @test von_neumann_entropy(rho) ≈ -sum(p .* log.(p)) rtol=1e-12

    reg = zero_state(4; nlevel=3)
    r = density_matrix(reg, [1,2])
    @test nqudits(r) == 2
end


text:
module Utils

## DATETIME

using TimeZones

mstodatetime(ms::Int) = Dates.DateTime(Dates.UTInstant(Dates.Millisecond(ms)))
daystodate(days::Int) = Dates.Date(Dates.UTInstant(Dates.Day(days)))
unixtime(seconds::Int) = DateTime(1970) + Dates.Second(seconds)
unixtimems(ms::Int) = DateTime(1970) + Dates.Millisecond(ms)

function tzconvert(datetime::DateTime,from::String,to::String)
    DateTime(astimezone(ZonedDateTime(datetime,TimeZone(from)),TimeZone(to)))
end

## CONTROL / OUTPUT

function lock_file(func::Function,fname::AbstractString;timeout=30)
    wait_begin = now()
    lock_fname = "$fname.lock"
    while isfile(lock_fname) && (now() - wait_begin) < Dates.Second(timeout)
        sleep(0.1)
    end
    if isfile(lock_fname)
        error("Unable to obtain lock on file \"$fname\"!")
    else
        open(lock_fname,"w") do f
            write(f,getpid())
        end
        func()

        isfile(lock_file) && rm(lock_fname)
    end
end

struct TimeoutException <: Exception
end

function timeout(expr,t::T) where {T<:Real}
    s = schedule(Task(expr))
    start_time = time()
    while (! istaskdone(s)) && (time() - start_time) < t
        sleep(0.1)
    end
    if ! istaskdone(s)
        s.state = :done
        throw(TimeoutException)
    end

    return yieldto(s)
end

# SIGNAL PROCESSING / STATS

z(p::AbstractArray{T,N}) where {T<:Real,N} = (p-mean(p))/std(p)

polyparams(x::AbstractArray{T,1},n::Integer) where {T<:Real} = [ float(x[i])^p for i = 1:length(x), p = 0:n ]
polyfit(y::AbstractArray{T,1},n::Integer) where {T<:Real} = polyparams(collect(1:length(y)),n) \ y
polyfitline(len::Integer,params::Array{Float64,1}) = polyparams(collect(1:len),length(params)-1) * params''
polyeval(x::Real,params::Array{Float64,1}) = [x^p for p=0:length(params)-1]' * params''

# Conversion / String

function smartparse(something)
    try
        return eval(parse(something))
    catch
        return something
    end
end

randascii(n) = join([rand(vcat('A':'Z','a':'z')) for i=1:n])

function objtodict(obj)
    Dict([(string(k),getfield(obj,k)) for k in fieldnames(obj)])
end

# List utilities

function cluster(a,dist;by=identity,sorted=false)
    sorted || (a = sort(a,by=by))
    da = [abs(by(a[i])-by(a[i+1])) for i=1:length(a)-1]
    clustered = typeof(a)[]
    push!(clustered,typeof(a)())
    push!(clustered[end],a[1])
    for i=2:length(a)
        da[i-1]>dist && push!(clustered,typeof(a)())
        push!(clustered[end],a[i])
    end
    return clustered
end

function clump(a,dist;by=x->x,zero=false)
    clustered = typeof(a)()
    i = 1
    while i<=length(a)
        if by(a[i])
            j = 1
            while i+j<=length(a) && by(a[i+j])
                j += 1
            end
            if j>=dist
                append!(clustered,a[i:i+j-1])
            else
                append!(clustered,fill(zero,j))
            end
            i += j
        else
            push!(clustered,a[i])
            i += 1
        end
    end
    return clustered
end

function getprecision(b)
    a = collect(b)
    return minimum([abs(a[i]-a[i+1]) for i in 1:length(a)-1])
end

_isrepeat(a::AbstractArray{T,1}) where {T<:Real} = vcat(1,diff(a)) .!= 0
uniqueinds(a::AbstractArray{T,1}) where {T<:Real} = findall(_isrepeat(a))
norepeat(a::AbstractArray{T,1}) where {T<:Real} = a[_isrepeat(a)]

# I don't know why I so much trouble doing this... this function converts an `Array` into an `Image` using a color map (a `Vector` of colors)

using Colors

function colormap_image(a::AbstractArray{T,2},m::Vector{K},scale::L=maximum(a)) where {T<:Real,K<:Colorant,L<:Real}
    i = Array{K,2}(undef,size(a))
    fac = length(m)/scale
    for ii in CartesianIndices(a)
        i[ii] = m[max(1,round(Int,fac*a[ii]))]
    end
    return i
end

end


text:
module SampleChainsDynamicHMC

using Reexport
@reexport using SampleChains
@reexport using DynamicHMC
using LogDensityProblems
using NestedTuples
using ElasticArrays
using StructArrays
using ConcreteStructs
using TransformVariables
using MappedArrays
using Random
using TupleVectors

using TupleVectors:chainvec

export dynamichmc

@concrete mutable struct DynamicHMCChain{T} <: AbstractChain{T}
    samples     # :: AbstractVector{T}
    logq        # log-density for distribution the sample was drawn from
    info        # Per-sample metadata, type depends on sampler used
    meta        # Metadata associated with the sample as a whole
    state
    transform
    reporter
end

function DynamicHMCChain(t::TransformVariables.TransformTuple, Q::DynamicHMC.EvaluatedLogDensity, tree_stats, steps, reporter = NoProgressReport())
    tQq = TransformVariables.transform(t, Q.q)
    T = typeof(tQq)
    samples = chainvec(tQq)
    logq = chainvec(Q.ℓq)
    info = chainvec(tree_stats)
    meta = steps
    transform = t

    return DynamicHMCChain{T}(samples, logq, info, meta, Q, transform, reporter)
end

TupleVectors.summarize(ch::DynamicHMCChain) = summarize(samples(ch))

function gettransform(chain::DynamicHMCChain)
    return getfield(chain, :transform)
end

function SampleChains.pushsample!(chain::DynamicHMCChain, Q::DynamicHMC.EvaluatedLogDensity, tree_stats)
    push!(samples(chain), transform(gettransform(chain), Q.q))
    push!(logq(chain), Q.ℓq)
    push!(info(chain), tree_stats)
end

function SampleChains.step!(chain::DynamicHMCChain)
    Q, tree_stats = DynamicHMC.mcmc_next_step(getfield(chain, :meta), getfield(chain, :state))
    setfield!(chain, :state, Q)
    return Q, tree_stats
end

@concrete struct DynamicHMCConfig <: ChainConfig{DynamicHMCChain}
    init
    warmup_stages
    algorithm
    reporter
    ad_backend
end

# Docs adapted from https://tamaspapp.eu/DynamicHMC.jl/stable/interface/
"""
    dynamichmc(
      ; init          = ()
      , warmup_stages = DynamicHMC.default_warmup_stages()
      , algorithm     = DynamicHMC.NUTS()
      , reporter      = DynamicHMC.NoProgressReport()
      , ad_backend    = Val(:ForwardDiff)
    )

`init`: a `NamedTuple` that can contain the following fields (all of them
optional and provided with reasonable defaults):
- `q`: initial position. Default: random (uniform `[-2,2]` for each coordinate).
- `κ`: kinetic energy specification. Default: Gaussian with identity matrix.
- `ϵ`: a scalar for initial step size, or `nothing` for heuristic finders.

`warmup_stages`: a sequence of warmup stages. See
`DynamicHMC.default_warmup_stages` and
`DynamicHMC.fixed_stepsize_warmup_stages`; the latter requires an `ϵ` in
initialization.

`algorithm`: see `DynamicHMC.NUTS`. It is very unlikely you need to modify this,
except perhaps for the maximum depth.

`reporter`: how progress is reported. This is currently silent by default (see
`DynamicHMC.NoProgressReport`), but this default will likely change in future
releases.

`ad_backend`: The automatic differentiation backend to use for gradient
computation, specified as either a symbol or a `Val` type with a symbol that
refers to an AD package. See [LogDensityProblems.jl](https://tamaspapp.eu/LogDensityProblems.jl/stable/#Automatic-differentiation)
for supported packages, including `ForwardDiff`, `ReverseDiff`, `Zygote`, and `Tracker`.

For more details see https://tamaspapp.eu/DynamicHMC.jl/stable/interface/
# Example

```jldoctest
julia> using LinearAlgebra, ReverseDiff

julia> config = dynamichmc(
           warmup_stages=default_warmup_stages(
               M=Symmetric, # adapt dense positive definite metric
               stepsize_adaptation=DualAveraging(δ=0.9), # target acceptance rate 0.9
               doubling_stages=7, # 7-stage metric adaptation
           ),
           reporter=LogProgressReport(), # log progress using Logging
           ad_backend=Val(:ReverseDiff), # use ReverseDiff AD package
       );
```
"""
function dynamichmc(;
    init          = ()
  , warmup_stages = DynamicHMC.default_warmup_stages()
  , algorithm     = DynamicHMC.NUTS()
  , reporter      = DynamicHMC.NoProgressReport()
  , ad_backend    = Val(:ForwardDiff)
)
    DynamicHMCConfig(init, warmup_stages, algorithm, reporter, ad_backend)
end

function SampleChains.newchain(rng::Random.AbstractRNG, config::DynamicHMCConfig, ℓ, tr)
    P = LogDensityProblems.TransformedLogDensity(tr, ℓ)
    ∇P = LogDensityProblems.ADgradient(config.ad_backend, P)
    reporter = config.reporter

    results = DynamicHMC.mcmc_keep_warmup(rng, ∇P, 0;
          initialization = config.init
        , warmup_stages  = config.warmup_stages
        , algorithm      = config.algorithm
        , reporter       = reporter
        )

    steps = DynamicHMC.mcmc_steps(
        results.sampling_logdensity,
        results.final_warmup_state,
    )

    Q = results.final_warmup_state.Q
    Q, tree_stats = DynamicHMC.mcmc_next_step(steps, Q)
    chain = DynamicHMCChain(tr, Q, tree_stats, steps, reporter)
end

function SampleChains.newchain(config::DynamicHMCConfig, ℓ, tr)
    return newchain(Random.GLOBAL_RNG, config, ℓ, tr)
end

function SampleChains.sample!(chain::DynamicHMCChain, n::Int=1000)
    reporter = getfield(chain, :reporter)
    mcmc_reporter = DynamicHMC.make_mcmc_reporter(reporter, n; currently_warmup = false)
    @cleanbreak for j in 1:n
        Q, tree_stats = step!(chain)
        pushsample!(chain, Q, tree_stats)
        DynamicHMC.report(mcmc_reporter, j)
    end
    return chain
end

end


text:
using Interpolation

using Test: @test, @testset

@testset "Interpolation Tests" begin
    @testset "Simple Analytic Examples" begin
        @test isapprox(polyint([1.0, 2.0], [1.0, 2.0])(3.0), 3.0)
        @test isapprox(polyint([1.0, 2.0, 3.0], [1.0, 4.0, 9.0])(4.0), 16.0)
        @test isapprox([polyint([1.0, 2.0, 3.0], [1.0, 4.0, 9.0], 1)(4.0)...], [10.0, 14.0])
    end

    @testset "Random Third Order Polynomials" begin
        for i in 1:10
            a,b,c,d = randn(4) # Roots

            xs = randn(4)

            ys = Float64[]
            for x in xs
                push!(ys, d*(x-a)*(x-b)*(x-c))
            end

            x = randn()
            poly = d*(x-a)*(x-b)*(x-c)

            @test isapprox(polyint(xs, ys)(x), poly)
        end
    end
end


text:
using LinearAlgebra, SparseArrays, SuiteSparse

struct ForwardSensitivityJacobian{T,JJ<:AbstractMatrix{T}} <: AbstractMatrix{T}
  J::JJ
end
Base.parent(J::ForwardSensitivityJacobian) = J.J
Base.similar(J::ForwardSensitivityJacobian, ::Type{T}) where T = ForwardSensitivityJacobian(similar(parent(J), T))

struct ForwardSensitivityJacobianFactorization{T,F<:Factorization{T}} <: Factorization{T}
  factorization::F
end
LinearAlgebra.lu!(J::ForwardSensitivityJacobian) = ForwardSensitivityJacobianFactorization(lu!(parent(J)))
function LinearAlgebra.ldiv!(F::ForwardSensitivityJacobianFactorization, x)
  F = F.factorization
  n = size(F, 1)
  k = length(x)÷n
  @assert k*n == length(x)
  ldiv!(F, reshape(x, n, k))
  x
end

# This is only used for oop stiff solvers
default_factorize(A) = lu(A)

mutable struct LinSolveFactorize{F}
  factorization::F
  A
end
LinSolveFactorize(factorization) = LinSolveFactorize(factorization,nothing)
function (p::LinSolveFactorize)(x,A,b,update_matrix=false;kwargs...)
  if update_matrix
    p.A = p.factorization(A)
  end
  if typeof(p.A) <: SuiteSparse.UMFPACK.UmfpackLU || typeof(p.factorization) <: typeof(lu)
    ldiv!(x,p.A,b) # No 2-arg form for SparseArrays!
  else
    x .= b
    ldiv!(p.A,x)
  end
end
function (p::LinSolveFactorize)(::Type{Val{:init}},f,u0_prototype)
  LinSolveFactorize(p.factorization,nothing)
end
Base.resize!(p::LinSolveFactorize,i) = p.A = nothing

mutable struct LinSolveGPUFactorize{F,T}
  factorization::F
  A
  x_cache::T
end
LinSolveGPUFactorize(factorization=qr) = LinSolveGPUFactorize(factorization,nothing,nothing)
function (p::LinSolveGPUFactorize)(x,A,b,update_matrix=false;kwargs...)
  if update_matrix
    p.A = p.factorization(cuify(A))
  end
  ldiv!(p.x_cache,p.A,cuify(b))
  x .= Array(p.x_cache)
end
function (p::LinSolveGPUFactorize)(::Type{Val{:init}},f,u0_prototype)
  LinSolveGPUFactorize(p.factorization,nothing,cuify(u0_prototype))
end
Base.resize!(p::LinSolveGPUFactorize,i) = p.A = nothing

### Default Linsolve

# Try to be as smart as possible
# lu! if Matrix
# lu if sparse
# gmres if operator

mutable struct DefaultLinSolve
  A
  iterable
  openblas::Union{Bool, Nothing}
end
DefaultLinSolve() = DefaultLinSolve(nothing, nothing, nothing)
@noinline function checkreltol(reltol)
  if !(reltol isa Real)
    error("Non real valued reltol is not supported by the linear iterative solvers. To customize tolerances for the linear iterative solvers, use the syntax like `KenCarp3(linsolve=LinSolveGMRES(abstol=1e-16,reltol=1e-16))`.")
  end
  return reltol
end

function isopenblas()
    @static if VERSION < v"1.7beta"
      blas = BLAS.vendor()
      blas == :openblas64 || blas == :openblas
    else
      occursin("openblas", BLAS.get_config().loaded_libs[1].libname)
    end
end

function (p::DefaultLinSolve)(x,A::Union{AbstractMatrix,AbstractDiffEqOperator},b,update_matrix=false;reltol=nothing, kwargs...)
  if p.iterable isa Vector && eltype(p.iterable) <: LinearAlgebra.BlasInt # `iterable` here is the pivoting vector
    F = LU{eltype(A)}(A, p.iterable, zero(LinearAlgebra.BlasInt))
    ldiv!(x, F, b)
    return nothing
  end
  if update_matrix
    if A isa Matrix
      # if the user doesn't use OpenBLAS, we assume that is a better BLAS
      # implementation like MKL
      #
      # RecursiveFactorization seems to be consistantly winning below 100
      # https://discourse.julialang.org/t/ann-recursivefactorization-jl/39213
      if ArrayInterface.can_setindex(x) && p.openblas === nothing
        # cache it because it's more expensive now
        p.openblas = isopenblas()
      end

      if ArrayInterface.can_setindex(x) && (size(A,1) <= 100 || (p.openblas && size(A,1) <= 500))
        p.A = RecursiveFactorization.lu!(A)
      else
        p.A = lu!(A)
      end
    elseif A isa Union{Tridiagonal, ForwardSensitivityJacobian}
      p.A = lu!(A)
    elseif A isa Union{SymTridiagonal}
      p.A = ldlt!(A)
    elseif A isa Union{Symmetric,Hermitian}
      p.A = bunchkaufman!(A)
    elseif A isa SparseMatrixCSC
      p.A = lu(A)
    elseif ArrayInterface.isstructured(A)
      p.A = factorize(A)
    elseif !(A isa AbstractDiffEqOperator)
      # Most likely QR is the one that is overloaded
      # Works on things like CuArrays
      p.A = qr(A)
    end
  end

  if A isa Union{Matrix,SymTridiagonal,Tridiagonal,Symmetric,Hermitian,ForwardSensitivityJacobian} # No 2-arg form for SparseArrays!
    copyto!(x,b)
    ldiv!(p.A,x)
  # Missing a little bit of efficiency in a rare case
  #elseif A isa DiffEqArrayOperator
  #  ldiv!(x,p.A,b)
  elseif ArrayInterface.isstructured(A) || A isa SparseMatrixCSC
    ldiv!(x,p.A,b)
  elseif A isa AbstractDiffEqOperator
    # No good starting guess, so guess zero
    if p.iterable === nothing
      reltol = checkreltol(reltol)
      p.iterable = IterativeSolvers.gmres_iterable!(x,A,b;initially_zero=true,restart=5,maxiter=5,abstol=1e-16,reltol=reltol,kwargs...)
    end
    fill!(x,false)
    iter = p.iterable
    purge_history!(iter, x, b)

    for residual in iter
    end
  else
    ldiv!(x,p.A,b)
  end
  return nothing
end

function (p::DefaultLinSolve)(::Type{Val{:init}},f,u0_prototype)
  if has_Wfact(f) || has_Wfact_t(f)
    piv = collect(one(LinearAlgebra.BlasInt):convert(LinearAlgebra.BlasInt, length(u0_prototype))) # pivoting vector
    DefaultLinSolve(f, piv, nothing)
  else
    DefaultLinSolve()
  end
end

Base.resize!(p::DefaultLinSolve,i) = p.A = nothing
const DEFAULT_LINSOLVE = DefaultLinSolve()

## A much simpler LU for when we know it's Array

mutable struct LUFactorize
  A::LU{Float64,Matrix{Float64}}
  openblas::Bool
end
LUFactorize() = LUFactorize(lu(rand(1,1)),isopenblas())
function (p::LUFactorize)(x::Vector{Float64},A::Matrix{Float64},b::Vector{Float64},update_matrix::Bool=false;kwargs...)
  if update_matrix
    if ArrayInterface.can_setindex(x) && (size(A,1) <= 100 || (p.openblas && size(A,1) <= 500))
      p.A = RecursiveFactorization.lu!(A)
    else
      p.A = lu!(A)
    end
  end
  ldiv!(x,p.A,b)
end
function (p::LUFactorize)(::Type{Val{:init}},f,u0_prototype)
  LUFactorize(lu(rand(eltype(u0_prototype),1,1)),p.openblas)
end
Base.resize!(p::LUFactorize,i) = p.A = nothing

### Default GMRES

# Easily change to GMRES

mutable struct LinSolveIterativeSolvers{F,PL,PR,AR,A}
  generate_iterator::F
  iterable
  Pl::PL
  Pr::PR
  args::AR
  kwargs::A
end
LinSolveIterativeSolvers(generate_iterator,args...;
                         Pl=IterativeSolvers.Identity(),
                         Pr=IterativeSolvers.Identity(),
                         kwargs...) =
                         LinSolveIterativeSolvers(generate_iterator, nothing, Pl, Pr, args, kwargs)

LinSolveGMRES(args...;kwargs...) = LinSolveIterativeSolvers(IterativeSolvers.gmres_iterable!,args...;kwargs...)
LinSolveCG(args...;kwargs...) = LinSolveIterativeSolvers(IterativeSolvers.cg_iterator!,args...;kwargs...)
LinSolveBiCGStabl(args...;kwargs...) = LinSolveIterativeSolvers(IterativeSolvers.bicgstabl_iterator!,args...;kwargs...)
LinSolveChebyshev(args...;kwargs...) = LinSolveIterativeSolvers(IterativeSolvers.chebyshev_iterable!,args...;kwargs...)
LinSolveMINRES(args...;kwargs...) = LinSolveIterativeSolvers(IterativeSolvers.minres_iterable!,args...;kwargs...)

function (f::LinSolveIterativeSolvers)(x,A,b,update_matrix=false; Pl=nothing, Pr=nothing, reltol=eps(eltype(x)), kwargs...)
  if f.iterable === nothing
    Pl = ComposePreconditioner(f.Pl, Pl, true)
    Pr = ComposePreconditioner(f.Pr, Pr, false)

    reltol = checkreltol(reltol)
    f.iterable = f.generate_iterator(x,A,b,f.args...;
                                     initially_zero=true,restart=5,
                                     maxiter=5,abstol=1e-16,reltol=reltol,
                                     Pl=Pl,Pr=Pr,
                                     kwargs...,f.kwargs...)
  end
  x .= false
  iter = f.iterable
  purge_history!(iter, x, b)

  for residual in iter
  end
  return nothing
end

function (p::LinSolveIterativeSolvers)(::Type{Val{:init}},f,u0_prototype)
  LinSolveIterativeSolvers(p.generate_iterator, nothing, p.Pl, p.Pr, p.args, p.kwargs)
end

# scaling for iterative solvers
struct ScaleVector{T}
  x::T
  isleft::Bool
end
function LinearAlgebra.ldiv!(v::ScaleVector, x)
  vx = vec(v.x)
  if v.isleft
    return @.. x = x * vx
  else
    return @.. x = x / vx
  end
end
function LinearAlgebra.ldiv!(y, v::ScaleVector, x)
  vx = vec(v.x)
  if v.isleft
    return @.. y = x * vx
  else
    return @.. y = x / vx
  end
end

struct ComposePreconditioner{P, S<:Union{ScaleVector,Nothing}}
  P::P
  scale::S
  isleft::Bool
end

function LinearAlgebra.ldiv!(v::ComposePreconditioner, x)
  isid = v.P isa IterativeSolvers.Identity
  isid || ldiv!(v.P, x)
  s = v.scale
  s === nothing && return x
  ldiv!(s, x)
  return x
end

function LinearAlgebra.ldiv!(y, v::ComposePreconditioner, x)
  isid = v.P isa IterativeSolvers.Identity
  isid || ldiv!(y, v.P, x)
  s = v.scale
  s === nothing && return x
  if isid
    ldiv!(y, s, x)
  else
    if v.isleft
      @.. y = y * s.x
    else
      @.. y = y / s.x
    end
  end
  return y
end

function purge_history!(iter::IterativeSolvers.GMRESIterable, x, b)
  iter.k = 1
  iter.x  = x
  iter.b  = b

  iter.residual.current = IterativeSolvers.init!(iter.arnoldi, iter.x, iter.b, iter.Pl, iter.Ax, initially_zero = true)
  IterativeSolvers.init_residual!(iter.residual, iter.residual.current)
  iter.β = iter.residual.current
  nothing
end

function Base.resize!(f::LinSolveIterativeSolvers,i)
    f.iterable = nothing
end


text:
#=
function fillzeroutri!(a::AbstractArray{T}) where T
    s = size(a,1)
    if s == 1 return @inbounds a[1,1] = zero(T) end
    @simd for m = 1:s
        @simd for n = m:s
            @inbounds a[m,n] = zero(T)
        end
    end
    a
end
=#
#=
function logerror!(e, lmm)
    if isa(e, DomainError)
        lmmlog!(lmm, LMMLogMsg(:ERROR, "DomainError ($(e.val), $(e.msg)) during REML calculation."))
    elseif isa(e, BoundsError)
        lmmlog!(lmm, LMMLogMsg(:ERROR, "BoundsError ($(e.a), $(e.i)) during REML calculation."))
    elseif isa(e, ArgumentError)
        lmmlog!(lmm, LMMLogMsg(:ERROR, "ArgumentError ($(e.msg)) during REML calculation."))
    elseif isa(e, LinearAlgebra.SingularException)
        lmmlog!(lmm, LMMLogMsg(:ERROR, "SingularException ($(e.info)) during REML calculation."))
    elseif isa(e, MethodError)
        lmmlog!(lmm, LMMLogMsg(:ERROR, "MethodError ($(e.f), $(e.args), $(e.world)) during REML calculation."))
    else
        lmmlog!(lmm, LMMLogMsg(:ERROR, "Unknown error during REML calculation."))
    end
end
=#

################################################################################
#                     β calculation
################################################################################
#=
function sweep_β(lmm, data::AbstractLMMDataBlocks, θ::Vector{T}) where T <: Number
    n             = length(lmm.covstr.vcovblock)
    θ₂            = zeros(T, lmm.rankx, lmm.rankx)
    βm            = zeros(T, lmm.rankx)
    β             = Vector{T}(undef, lmm.rankx)
    #---------------------------------------------------------------------------
    akk           = zeros(T, lmm.covstr.maxn + lmm.rankx) #temp for sweep
    Vm            = Matrix{T}(undef, lmm.covstr.maxn + lmm.rankx, lmm.covstr.maxn + lmm.rankx) #!!
    @inbounds @simd for i = 1:n
        q    = length(lmm.covstr.vcovblock[i])
        qswm = q + lmm.rankx
        Vp   = view(Vm, 1:q + lmm.rankx, 1:q + lmm.rankx)
        V    = view(Vm, 1:q, 1:q)
        fillzeroutri!(V)
        Vx   = view(Vm, 1:q, q+1:q+lmm.rankx)
        copyto!(Vx, data.xv[i])
        Vc   = view(Vm, q + 1:qswm, q + 1:qswm)
        fillzeroutri!(Vc)
        vmatrix!(V, θ, lmm, i)
        #-----------------------------------------------------------------------
        sweepb!(fill!(view(akk, 1:qswm), zero(T)), Vp, 1:q)
        subutri!(θ₂, view(Vp, q + 1:qswm, q + 1:qswm))
        mulαtβinc!(βm, view(Vp, 1:q, q + 1:qswm), data.yv[i])
    end
    mul!(β, inv(Symmetric(θ₂)), βm)
    return  β
end
=#

#=
function gmat_zero!(mx, θ::Vector{T}, ::Int, ::CovarianceType) where T
    mx .= zero(T)
    nothing
end
=#
#=
function lcontrast(lmm::LMM, i::Int)
    n = nterms(lmm.mf)
    if i > n || n < 1 error("Factor number out of range 1-$(n)") end
    inds = findall(x -> x==i, lmm.mm.assign)
    mx = zeros(length(inds), size(lmm.mm.m, 2))
    for i = 1:length(inds)
        mx[i, inds[i]] = 1
    end
    mx
end
=#
################################################################################
# Intersect dataframe.
################################################################################
#=
function intersectdf(df, s)::Vector
    if isa(s, Nothing) return [collect(1:size(df, 1))] end
    if isa(s, Symbol) s = [s] end
    if length(s) == 0 return [collect(1:size(df, 1))] end
    u   = unique(@view df[:, s])
    sort!(u, s)
    res = Vector{Vector{Int}}(undef, size(u, 1))
    v   = Vector{Dict{}}(undef, size(u, 2))
    v2  = Vector{Vector{Int}}(undef, size(u, 2))
    for i2 = 1:size(u, 2)
        uv = unique(@view u[:, i2])
        v[i2] = Dict{Any, Vector{Int}}()
        for i = 1:length(uv)
            v[i2][uv[i]] = findall(x -> x == uv[i], @view df[:,  s[i2]])
        end
    end
    for i2 = 1:size(u, 1)
        for i = 1:length(s)
            v2[i] = v[i][u[i2, i]]
        end
        res[i2] = collect(intersect(Set.(v2)...))
        #res[i2] = intersect(v2...)
        sort!(res[i2])
    end
    res
end
################################################################################
# Intersect subject set in effects.
################################################################################
function intersectsubj(random, repeated)
    a  = Vector{Vector{Symbol}}(undef, length(random)+1)
    eq = true
    for i = 1:length(random)
        a[i] = random[i].subj
    end
    a[end] = repeated.subj
    for i = 2:length(a)
        if !(issetequal(a[1], a[i]))
            eq = false
            break
        end
    end
    intersect(a...), eq
end
function intersectsubj(random)
    if !isa(random, Vector) return random.subj end
    a  = Vector{Vector{Symbol}}(undef, length(random))
    for i = 1:length(random)
        a[i] = random[i].subj
    end
    intersect(a...)
end
=#
#=
function varlinkvec(v)
    fv = Vector{Function}(undef, length(v))
    for i = 1:length(v)
        if v[i] == :var fv[i] = vlink else fv[i] = rholinksigmoid end
    end
    fv
end
function varlinkrvec(v)
    fv = Vector{Function}(undef, length(v))
    for i = 1:length(v)
        if v[i] == :var fv[i] = vlinkr else fv[i] = rholinksigmoidr end
    end
    fv
end
=#
#=
function makepmatrix(m::Matrix)
    sm = string.(m)
    lv = maximum(length.(sm), dims = 1)
    for r = 1:size(sm, 1)
        for c = 1:size(sm, 2)
            sm[r,c] = addspace(sm[r,c], lv[c] - length(sm[r,c]))*"   "
        end
    end
end
=#
#=
function gmat(θ::Vector{T}, zn::Int, ::CovarianceType, ::Val{:SI}) where T
    Matrix{T}(I(zn)*(θ[1] ^ 2))
    #I(zn)*(θ[1] ^ 2)
end
function gmat(θ::Vector{T}, ::Int, ::CovarianceType, ::Val{:VC}) where T
    Matrix{T}(Diagonal(θ .^ 2))
    #Diagonal(θ .^ 2)
end
function gmat(θ::Vector{T}, zn::Int, ::CovarianceType, ::Val{:AR}) where T
    mx  = Matrix{T}(undef, zn, zn)
    mx .= θ[1] ^ 2
    if zn > 1
        for m = 1:zn - 1
            for n = m + 1:zn
                @inbounds mx[m, n] = mx[m, m] * θ[2] ^ (n - m)
            end
        end
    end
    Matrix{T}(Symmetric(mx))
    #Symmetric(mx)
end
function gmat(θ::Vector{T}, zn::Int, ::CovarianceType, ::Val{:ARH}) where T
    mx  = Matrix{T}(undef, zn, zn)
    for m = 1:zn
        @inbounds mx[m, m] = θ[m]
    end
    if zn > 1
        for m = 1:zn - 1
            for n = m + 1:zn
                @inbounds mx[m, n] = mx[m, m] * mx[n, n] * θ[end] ^ (n - m)
            end
        end
    end
    for m = 1:zn
        @inbounds mx[m, m] = mx[m, m] * mx[m, m]
    end
    Matrix{T}(Symmetric(mx))
    #Symmetric(mx)
end
function gmat(θ::Vector{T}, zn::Int, ::CovarianceType, ::Val{:CSH}) where T
    mx = Matrix{T}(undef, zn, zn)
    for m = 1:zn
        @inbounds mx[m, m] = θ[m]
    end
    if zn > 1
        for m = 1:zn - 1
            for n = m + 1:zn
                @inbounds mx[m, n] = mx[m, m] * mx[n, n] * θ[end]
            end
        end
    end
    for m = 1:zn
        @inbounds mx[m, m] = mx[m, m] * mx[m, m]
    end
    Matrix{T}(Symmetric(mx))
    #Symmetric(mx)
end
=#
#=
function gmat_blockdiag(θ::Vector{T}, covstr) where T
    vm = Vector{AbstractMatrix{T}}(undef, length(covstr.ves) - 1)
    for i = 1:length(covstr.random)
        vm[i] = gmat(θ[covstr.tr[i]], covstr.q[i], covstr.random[i].covtype, Val{covstr.random[i].covtype.s}()) #covstr.random[i])
    end
    BlockDiagonal(vm)
end
=#
#=
@inline function rmat(θ::Vector{T}, rz, rn, ct, ::Val{:SI}) where T
    I(rn) * (θ[1] ^ 2)
end
@inline function rmat(θ::Vector{T}, rz, rn, ct, ::Val{:VC}) where T
    Diagonal(rz * (θ .^ 2))
end
@inline function rmat(θ::Vector{T}, rz, rn, ct, ::Val{:AR}) where T
    mx  = Matrix{T}(undef, rn, rn)
    mx .= θ[1] ^ 2
    if rn > 1
        for m = 1:rn - 1
            for n = m + 1:rn
                @inbounds mx[m, n] = mx[m, m] * θ[2] ^ (n - m)
            end
        end
    end
    Symmetric(mx)
end
@inline function rmat(θ::Vector{T}, rz, rn, ct, ::Val{:ARH}) where T
    mx   = Matrix(Diagonal(rz * (θ[1:end-1])))
    if rn > 1
        for m = 1:rn - 1
            for n = m + 1:rn
                @inbounds mx[m, n] = mx[m, m] * mx[n, n] * θ[end] ^ (n - m)
            end
        end
    end
    for m = 1:rn
        @inbounds mx[m, m] = mx[m, m] * mx[m, m]
    end
    Symmetric(mx)
end
@inline function rmat(θ::Vector{T}, rz, rn,  ct, ::Val{:CSH}) where T #???
    mx   = Matrix(Diagonal(rz * (θ[1:end-1])))
    if rn > 1
        for m = 1:rn - 1
            for n = m + 1:rn
                @inbounds mx[m, n] = mx[m, m] * mx[n, n] * θ[end]
            end
        end
    end
    for m = 1:rn
        @inbounds mx[m, m] = mx[m, m] * mx[m, m]
    end
    Symmetric(mx)
end
=#
#=
function get_z_matrix(data, covstr::CovStructure{Vector{VarEffect}})
    rschema = apply_schema(covstr.random[1].model, schema(data, covstr.random[1].coding))
    Z       = modelcols(rschema, data)
    if length(covstr.random) > 1
        for i = 1:length(covstr.random)
            rschema = apply_schema(covstr.random[i].model, schema(data, covstr.random[i].coding))
            Z       = hcat(modelcols(rschema, data))
        end
    end
    Z
end

function get_z_matrix(data, covstr::CovStructure)
    rschema = apply_schema(covstr.random.model, schema(data, covstr.random.coding))
    Z       = modelcols(rschema, data)
end

function get_term_vec(covstr::CovStructure)
    covstr.random.model
end
=#
#=
"""

"""
#cfg = ForwardDiff.JacobianConfig(covmat, [1.0,2.0,3.0]);
#function covmat_grad(f, θ, cfg)
#    ForwardDiff.jacobian(f, θ, cfg);
#end
function covmat_grad(f, Z, θ)
    #fx = x -> f(gmat(x[3:5]), rmat(x[1:2], Z), Z)
    #cfg   = ForwardDiff.JacobianConfig(fx, θ)
    m     = ForwardDiff.jacobian(x -> f(gmat(x[3:5]), rmat(x[1:2], Z), Z), θ);
    reshape(m, size(Z, 1), size(Z, 1), length(θ))
end
function covmat_hessian(f, θ)
    ForwardDiff.jacobian(x -> ForwardDiff.jacobian(f, x), θ)
end
=#
################################################################################
#=
function covmat_grad2(f, Z, θ)
    #fx = x -> f(gmat(x[3:5]), rmat(x[1:2], Z), Z)
    #cfg   = ForwardDiff.JacobianConfig(fx, θ)

    m     = ForwardDiff.jacobian(x -> f(gmat(x[3:5]), Z, x), θ);
end

function fgh!(F, G, H, x; remlβcalc::Function, remlcalc::Function)

  reml, β, c = remlβcalc(x)
  remlcalcd = x -> remlcalc(β, x)
  if G != nothing
      G .= ForwardDiff.gradient(remlcalcd, x)
  end
  if H != nothing
      H .= ForwardDiff.hessian(remlcalcd, x)
  end
  if F != nothing
    return reml
  end
  nothing
end
=#

"""
    Make X, Z matrices and vector y for each subject;
"""
#=
function subjblocks(df, sbj::Symbol, x::Matrix{T}, z::Matrix{T}, y::Vector{T}, rz::Matrix{T}) where T
    u = unique(df[!, sbj])
    xa  = Vector{Matrix{T}}(undef, length(u))
    za  = Vector{Matrix{T}}(undef, length(u))
    ya  = Vector{Vector{T}}(undef, length(u))
    rza = Vector{Matrix{T}}(undef, length(u))
        @inbounds @simd for i = 1:length(u)
            v = findall(x -> x == u[i], df[!, sbj])
            xa[i] = Matrix(view(x, v, :))
            za[i] = Matrix(view(z, v, :))
            ya[i] = Vector(view(y, v))
            rza[i] = Matrix(view(rz, v, :))
        end
    return xa, za, rza, ya
end
function subjblocks(df, sbj::Symbol, x::Matrix{T}, z::Matrix{T}, y::Vector{T}, rz::Nothing) where T
    u = unique(df[!, sbj])
    xa  = Vector{Matrix{T}}(undef, length(u))
    za  = Vector{Matrix{T}}(undef, length(u))
    ya  = Vector{Vector{T}}(undef, length(u))
    rza = Vector{Matrix{T}}(undef, length(u))
        @inbounds @simd for i = 1:length(u)
            v = findall(x -> x == u[i], df[!, sbj])
            xa[i]  = Matrix(view(x, v, :))
            za[i]  = Matrix(view(z, v, :))
            ya[i]  = Vector(view(y, v))
            rza[i] = Matrix{T}(undef, 0, 0)
        end
    return xa, za, rza, ya
end
function subjblocks(df, sbj)
    if isa(sbj, Symbol)
        u = unique(df[!, sbj])
        r = Vector{Vector{Int}}(undef, length(u))
        @inbounds @simd for i = 1:length(u)
            r[i] = findall(x -> x == u[i], df[!, sbj])
        end
        return r
    end
    r = Vector{Vector{Int}}(undef, 1)
    r[1] = collect(1:size(df, 1))
    r
end
=#

#=
function intersectsubj(covstr)
    a  = Vector{Vector{Symbol}}(undef, length(covstr.random)+1)
    eq = true
    for i = 1:length(covstr.random)
        a[i] = covstr.random[i].subj
    end
    a[end] = covstr.repeated.subj
    for i = 2:length(a)
        if !(issetequal(a[1], a[i]))
            eq = false
            break
        end
    end
    intersect(a...), eq
end
=#
#=
function diffsubj!(a, subj)
    push!(a, subj)
    symdiff(a...)
end
=#
#=
function varlinkvecapply(v, f)
    rv = similar(v)
    for i = 1:length(v)
        rv[i] = f[i](v[i])
    end
    rv
end

function varlinkvecapply!(v, f)
    for i = 1:length(v)
        v[i] = f[i](v[i])
    end
    v
end
=#
#varlinkvecapply(v, f) = map.(f,v) #Test

#=
function varlinkrvecapply2(v, p; varlinkf = :exp, rholinkf = :sigm)
    s = similar(v)
    for i = 1:length(v)
        if p[i] == :var
            s[i] = vlinkr(v[i])
        else
            s[i] = rholinksigmoidr(v[i])
        end
    end
    s
end
=#
################################################################################
#=
function vmatr(lmm, i)
    θ  = lmm.result.theta
    G  = gmat_base(θ, lmm.covstr)
    V  = mulαβαt(view(lmm.data.zv, lmm.data.block[i],:), G)
    if length(lmm.data.zrv) > 0
        rmat_basep!(V, θ[lmm.covstr.tr[end]], view(lmm.data.zrv, lmm.data.block[i],:), lmm.covstr)
    else
        rmat_basep!(V, θ[lmm.covstr.tr[end]], lmm.data.zrv, lmm.covstr)
    end
    V
end

function gmatr(lmm, i)
    θ  = lmm.result.theta
    gmat_base(θ, lmm.covstr)
end
=#

#=
function rmat_basep_z!(mx, θ::AbstractVector{T}, zrv, covstr) where T
    for i = 1:length(covstr.block[end])
        if covstr.repeated.covtype.s == :SI
            rmatp_si!(view(mx, covstr.block[end][i], covstr.block[end][i]), θ, view(covstr.rz, covstr.block[end][i], :), covstr.repeated.covtype)
        elseif covstr.repeated.covtype.s == :DIAG
            rmatp_diag!(view(mx, covstr.block[end][i], covstr.block[end][i]), θ, view(covstr.rz, covstr.block[end][i], :), covstr.repeated.covtype)
        elseif covstr.repeated.covtype.s == :AR
            rmatp_ar!(view(mx, covstr.block[end][i], covstr.block[end][i]), θ, view(covstr.rz, covstr.block[end][i], :), covstr.repeated.covtype)
        elseif covstr.repeated.covtype.s == :ARH
            rmatp_arh!(view(mx, covstr.block[end][i], covstr.block[end][i]), θ, view(covstr.rz, covstr.block[end][i], :), covstr.repeated.covtype)
        elseif covstr.repeated.covtype.s == :CSH
            rmatp_csh!(view(mx, covstr.block[end][i], covstr.block[end][i]), θ, view(covstr.rz, covstr.block[end][i], :), covstr.repeated.covtype)
        elseif covstr.repeated.covtype.s == :CS
            rmatp_cs!(view(mx, covstr.block[end][i], covstr.block[end][i]), θ, view(covstr.rz, covstr.block[end][i], :), covstr.repeated.covtype)
        else
            error("Unknown covariance structure!")
        end
    end
    mx
end
=#

"""
    -2 log Restricted Maximum Likelihood;
"""
#=
function reml_sweep(lmm, β, θ::Vector{T})::T where T <: Number
    n  = length(lmm.data.yv)
    N  = sum(length.(lmm.data.yv))
    G  = gmat_base(θ, lmm.covstr)
    c  = (N - lmm.rankx)*log(2π)
    θ₁ = zero(eltype(θ))
    θ₂ = zero(eltype(θ))
    θ₃ = zero(eltype(θ))
    θ2m  = zeros(eltype(θ), lmm.rankx, lmm.rankx)
    @inbounds for i = 1:n
        q   = length(lmm.data.yv[i])
        Vp  = mulαβαt3(lmm.data.zv[i], G, lmm.data.xv[i])
        V   = view(Vp, 1:q, 1:q)
        rmat_basep!(V, θ[lmm.covstr.tr[end]], lmm.data.zrv[i], lmm.covstr)

        θ₁  += logdet(V)
        sweep!(Vp, 1:q)
        iV  = Symmetric(utriaply!(x -> -x, Vp[1:q, 1:q]))
        mulαtβαinc!(θ2m, lmm.data.xv[i], iV)
        θ₃  += -Vp[end, end]
    end
    θ₂       = logdet(θ2m)
    return   -(θ₁ + θ₂ + θ₃ + c)
end
=#

#=
function gfunc!(g, x, f)
    chunk  = ForwardDiff.Chunk{1}()
    gcfg   = ForwardDiff.GradientConfig(f, x, chunk)
    ForwardDiff.gradient!(g, f, x, gcfg)
end
function hfunc!(h, x, f)
    chunk  = ForwardDiff.Chunk{1}()
    hcfg   = ForwardDiff.HessianConfig(f, x, chunk)
    ForwardDiff.hessian!(h, f, x, hcfg)
end
=#

"""
A * B * A' + C
"""
#=
function mulαβαtc(A, B, C)
    q  = size(B, 1)
    p  = size(A, 1)
    c  = zeros(eltype(B), q)
    mx = zeros(eltype(B), p, p)
    for i = 1:p
        fill!(c, zero(eltype(c)))
        @simd for n = 1:q
            @simd for m = 1:q
                @inbounds c[n] +=  A[i, m] * B[n, m]
            end
        end
        @simd for n = i:p
            @simd for m = 1:q
                 @inbounds mx[i, n] += A[n, m] * c[m]
            end
            @inbounds mx[i, n] += C[i, n]
        end
    end
    Symmetric(mx)
end
=#
"""
A * B * A'
"""
#=
function mulαβαt(A, B)
    q  = size(B, 1)
    p  = size(A, 1)
    c  = zeros(eltype(B), q)
    mx = zeros(eltype(B), p, p)
    for i = 1:p
        fill!(c, zero(eltype(c)))
        @simd for n = 1:q
            @simd for m = 1:q
                @inbounds c[n] +=  A[i, m] * B[n, m]
            end
        end
        @simd for n = i:p
            @simd for m = 1:q
                 @inbounds mx[i, n] += A[n, m] * c[m]
            end
        end
    end
    Symmetric(mx)
end
function mulαβαtc2(A, B, C, r::Vector)
    q  = size(B, 1)
    p  = size(A, 1)
    c  = zeros(eltype(B), q)
    mx = zeros(eltype(B), p + 1, p + 1)
    for i = 1:p
        fill!(c, zero(eltype(c)))
        @simd for n = 1:q
            @simd for m = 1:q
                @inbounds c[n] +=  A[i, m] * B[n, m]
            end
        end
        @simd for n = 1:p
            @simd for m = 1:q
                 @inbounds mx[i, n] += A[n, m] * c[m]
            end
            @inbounds mx[i, n] += C[i, n]
        end
        mx[end, i] = r[i]
        mx[i, end] = r[i]
    end
    mx
end
function mulαβαtc3(A, B, C, X)
    q  = size(B, 1)
    p  = size(A, 1)
    c  = zeros(eltype(B), q)
    mx = zeros(eltype(B), p + size(X, 2), p + size(X, 2))
    for i = 1:p
        fill!(c, zero(eltype(c)))
        @simd for n = 1:q
            @simd for m = 1:q
                @inbounds c[n] +=  A[i, m] * B[n, m]
            end
        end
        @simd for n = 1:p
            @simd for m = 1:q
                 @inbounds mx[i, n] += A[n, m] * c[m]
            end
            @inbounds mx[i, n] += C[i, n]
        end
    end
    mx[1:p, p+1:end] = X
    mx[p+1:end, 1:p] = X'
    mx
end
=#

"""
A' * B * A -> +θ
A' * B * C -> +β
"""
#=
function mulθβinc!(θ, β, A::AbstractMatrix, B::AbstractMatrix, C::AbstractVector)
    q = size(B, 1)
    p = size(A, 2)
    c = Vector{eltype(θ)}(undef, q)
    for i = 1:p
        fill!(c, zero(eltype(θ)))
        for n = 1:q
            for m = 1:q
                @inbounds c[n] += B[m, n] * A[m, i]
            end
            @inbounds β[i] += c[n] * C[n]
        end
        for n = 1:p
            for m = 1:q
                @inbounds θ[i, n] += A[m, n] * c[m]
            end
        end
    end
    θ, β
end
=#
#-------------------------------------------------------------------------------
"""
A' * B * A -> + θ
"""
#=
function mulαtβαinc!(θ, A, B)
    q = size(B, 1)
    p = size(A, 2)
    c = zeros(eltype(B), q)
    for i = 1:p
        fill!(c, zero(eltype(c)))
        @inbounds for n = 1:q, m = 1:q
            c[n] += B[m, n] * A[m, i]
        end
        @inbounds for n = 1:p, m = 1:q
            θ[i, n] += A[m, n] * c[m]
        end
    end
end
=#

"""
A' * B * A -> θ
"""
#=
function mulαtβα!(θ, A, B)
    q = size(B, 1)
    p = size(A, 2)
    c = zeros(eltype(B), q)
    fill!(θ, zero(eltype(θ)))
    for i = 1:p
        fill!(c, zero(eltype(c)))
        @inbounds for n = 1:q, m = 1:q
            c[n] += B[m, n] * A[m, i]
        end
        @inbounds for n = 1:p, m = 1:q
            θ[i, n] += A[m, n] * c[m]
        end
    end
    θ
end
=#
"""
A * B * A -> θ
"""
#=
function mulαβα!(θ, A, B)
    q = size(B, 1)
    p = size(A, 2)
    c = zeros(eltype(B), q)
    fill!(θ, zero(eltype(θ)))
    for i = 1:p
        fill!(c, zero(eltype(c)))
        @inbounds for n = 1:q, m = 1:q
            c[n] += B[m, n] * A[i, m]
        end
        @inbounds for n = 1:p, m = 1:q
            θ[i, n] += A[m, n] * c[m]
        end
    end
    θ
end
=#
"""
tr(A * B)
"""
#=
function trmulαβ(A, B)
    c = 0
    @inbounds for n = 1:size(A,1), m = 1:size(B, 1)
        c += A[n,m] * B[m, n]
    end
    c
end
=#
"""
tr(H * A' * B * A)
"""
#=
function trmulhαtβα(H, A, B)
end
=#

"""
(y - X * β)
"""
#=
function mulr!(v::AbstractVector, y::AbstractVector, X::AbstractMatrix, β::AbstractVector)
    fill!(v, zero(eltype(v)))
    q = length(y)
    p = size(X, 2)
    @simd for n = 1:q
        @simd for m = 1:p
            @inbounds v[n] += X[n, m] * β[m]
        end
        v[n] = y[n] - v[n]
    end
    return v
end
function mulr(y::AbstractVector, X::AbstractMatrix, β::AbstractVector)
    v = zeros(eltype(β), length(y))
    q = length(y)
    p = size(X, 2)
    @simd for n = 1:q
        @simd for m = 1:p
            @inbounds v[n] += X[n, m] * β[m]
        end
        v[n] = y[n] - v[n]
    end
    return v
end
=#


text:
#####
##### Activation
#####

#@test_throws MuJoCoException mj_activate("")
#@test_skip mj_activate("")

@testset "Parse and compile" begin
    pm = mj_loadXML(TESTMODELXML)
    pd = mj_makeData(pm)
    m = unsafe_load(pm)
    d = unsafe_load(pd)
    mj_resetData(m, d)
    mj_forward(m, d)

    pm2 = mj_loadXML(TESTMODELXML)
    pd2 = mj_makeData(pm2)
    m2 = unsafe_load(pm2)
    d2 = unsafe_load(pd2)
    mj_resetData(m2, d2)
    mj_forward(m2, d2)

    @test m !== m2
    @test d !== d2
end

@testset "Main simulation" begin
    m, d, jm, jd = jlmakemd()

    @test @inferred(mj_step(m, d)) === nothing
    @test @inferred(mj_step1(m, d)) === nothing
    @test @inferred(mj_step2(m, d)) === nothing

    @test @inferred(mj_forward(m, d)) === nothing
    @test @inferred(mj_inverse(m, d)) === nothing
    @test @inferred(mj_forwardSkip(m, d, MJCore.mjSTAGE_NONE, false)) === nothing
    @test @inferred(mj_inverseSkip(m, d, MJCore.mjSTAGE_NONE, false)) === nothing

    @test @inferred(mj_step(jm, jd)) === nothing
    @test @inferred(mj_step1(jm, jd)) === nothing
    @test @inferred(mj_step2(jm, jd)) === nothing
    @test @inferred(mj_forward(jm, jd)) === nothing
    @test @inferred(mj_inverse(jm, jd)) === nothing
    @test @inferred(mj_forwardSkip(jm, jd, MJCore.mjSTAGE_NONE, false)) === nothing
    @test @inferred(mj_inverseSkip(jm, jd, MJCore.mjSTAGE_NONE, false)) === nothing
end

@testset "Initialization" begin
    let pm = mj_loadXML(TESTMODELXML), pd = mj_makeData(pm)
        @test @inferred(MJCore.mj_deleteData(pd)) === nothing
        @test @inferred(MJCore.mj_deleteModel(pm)) === nothing
    end

    # load 4 models from: 1) two different mj_loadXML calls, 1 deepcopy, and 1 constructor call
    # all should all be equal.
    let
        pm1 = mj_loadXML(TESTMODELXML)
        pm2 = mj_copyModel(pm1)
        m1 = unsafe_load(pm1)
        m2 = unsafe_load(pm2)
        jm1 = jlModel(pm1)
        jm2 = jlModel(pm2)
        jm3 = deepcopy(jm1)
        jm4 = jlModel(jm1)

        @testset "jlModel: $name" for name in propertynames(jlModel)
            jf1 = getproperty(jm1, name)
            jf2 = getproperty(jm2, name)
            jf3 = getproperty(jm3, name)
            jf4 = getproperty(jm4, name)
            ret = jf1 == jf2 == jf3 == jf4
            if fieldtype(mjModel, name) <: Ptr
                f1 = getfield(m1, name)
                f2 = getfield(m2, name)
                ret &= (f1 != f2)
            end
            @test ret
        end
    end

    # load datas from 4 different ways: pointers to mjData and mjModel,
    # just mjModel, and from instance of jlModel
    let
        pm = mj_loadXML(TESTMODELXML)
        jm = jlModel(pm)

        pd1 = mj_makeData(pm)
        jd1 = jlData(pm, pd1)

        jd2 = jlData(pm)

        jd3 = jlData(jm)

        mj_step(pm, jd1)
        mj_step(pm, jd2)
        mj_step(pm, jd3)

        @testset "jlData: $name" for name in propertynames(jlData)
            jf1 = getproperty(jd1, name)
            jf2 = getproperty(jd2, name)
            jf3 = getproperty(jd3, name)
            test() = jf1 == jf2 == jf3

            if name === :solver_nnz
                # TODO
                @test_skip test()
            else
                @test test()
            end

            #if name === :solver_nnz
            #    # NOTE: bug in MuJoCo? d1.solver_nnz == 729,
            #    # but subsequent data's have solver_nnz == 0
            #    @test_broken test()
            #elseif name === :timer
            #    # jd1.timer has non zero entries for # number?
            #    @test_broken test()
            #else
            #    @test test()
            #end
        end
    end

    let jm = jlModel(TESTMODELXML), jd1 = jlData(jm), jd2 = jlData(jm)
        jd1.qpos .= 12345
        @test jd2.qpos != jd1.qpos
        mj_copyData(jd2, jm, jd1)
        @test jd2.qpos == jd1.qpos
    end
end