text:
"""
  compute the interior penalty matrix
  **Input**
   * mesh
   * sbp
   * eqn
   * opts
   * iface: the index of interface
  **Input/Output**
   * pMat: penalty matrix
"""
function cmptIPMat(mesh::AbstractMesh{Tmsh},
                   sbp::AbstractOperator,
                   eqn::NSData{Tsol, Tres, Tdim},
                   opts,
                   iface::Int,
                   GtL::AbstractArray{Tsol, 5},
                   GtR::AbstractArray{Tsol, 5},
                   pMat::AbstractArray{Tsol, 3}) where {Tmsh, Tdim, Tsol, Tres}
  if opts["SAT_type"] == "Hartman"
    cmptIPMat_hartman(mesh, sbp, eqn, opts, iface, GtL, GtR, pMat)
  elseif opts["SAT_type"] == "SAT-SIPG"
    cmptIPMat_SIPG(mesh, sbp, eqn, opts, iface, GtL, GtR, pMat)
  elseif opts["SAT_type"] == "SAT-BR2"
    cmptIPMat_BR2(mesh, sbp, eqn, opts, iface, GtL, GtR, pMat)
  end
end

function cmptIPMat_SIPG(mesh::AbstractMesh{Tmsh},
                        sbp::AbstractOperator,
                        eqn::NSData{Tsol, Tres, 2},
                        opts,
                        iface::Int,
                        GtL::AbstractArray{Tsol, 5},
                        GtR::AbstractArray{Tsol, 5},
                        pMat::AbstractArray{Tsol, 3}) where {Tmsh, Tsol, Tres}
  error("SAT-SIPG not available yet")
end

function cmptIPMat_BR2(mesh::AbstractMesh{Tmsh},
                       sbp::AbstractOperator,
                       eqn::NSData{Tsol, Tres, 2},
                       opts,
                       iface::Int,
                       GtL::AbstractArray{Tsol, 5},
                       GtR::AbstractArray{Tsol, 5},
                       pMat::AbstractArray{Tsol, 3}) where {Tmsh, Tsol, Tres}
  error("SAT-BR2 not available yet")
end

function cmptIPMat_hartman(mesh::AbstractMesh{Tmsh},
                           sbp::AbstractOperator,
                           eqn::NSData{Tsol, Tres, 2},
                           opts,
                           iface::Int,
                           GtL::AbstractArray{Tsol, 5},
                           GtR::AbstractArray{Tsol, 5},
                           pMat::AbstractArray{Tsol, 3}) where {Tmsh, Tsol, Tres}
# Reference:
#    Shahbazi, Mavriplis, Multigrid algorithms for high order discontinuous
#    Galerkin discretization of the compressible Navier-Stokes equations,
#    JCP, volume 228 Issue 21, November, 2009
#
  # in order to enhance/test stability, we try this factor on penalty terms
  factor = eqn.params.penalty_relaxation
  const_tii = eqn.params.const_tii
  Tdim = 2
  sbpface = mesh.sbpface

  face = mesh.interfaces[iface]
  elemL = face.elementL
  elemR = face.elementR

  # Compute geometric info on face
  nrm1 = zeros(Tmsh, Tdim, mesh.numNodesPerFace)
  area = zeros(Tmsh, mesh.numNodesPerFace)
  for n = 1 : mesh.numNodesPerFace
    nrm_xy = ro_sview(mesh.nrm_face, :, n, iface)
    area[n] = norm(nrm_xy)

    for i = 1 : Tdim
      nrm1[i,n] = nrm_xy[i] / area[n]
    end
  end

  # Compute the size of element and face, and then meas(face)/meas(elem)
  elem_volL = 0.0
  elem_volR = 0.0
  face_area = 0.0
  for n = 1:mesh.numNodesPerElement
    elem_volL +=  sbp.w[n]/mesh.jac[n, elemL]
    elem_volR +=  sbp.w[n]/mesh.jac[n, elemR]
  end

  for n = 1:mesh.numNodesPerFace
    face_area +=  sbpface.wface[n]*area[n]
  end

  heL = elem_volL/eqn.area_sum[elemL]
  heR = elem_volR/eqn.area_sum[elemR]
  he = min(heL, heR)

  for n = 1 : mesh.numNodesPerFace
    # for iDof = 1:mesh.numDofPerNode
      # for jDof = 1:mesh.numDofPerNode
        # pMat[iDof, jDof, n] = 0.0
        # for jDim = 1 : Tdim
          # for iDim = 1 : Tdim
            # pMat[iDof,jDof,n] +=  nrm1[iDim,n]*nrm1[jDim,n] * (GtL[iDof,jDof,iDim,jDim,n] + GtR[iDof,jDof,iDim,jDim,n])
          # end
        # end
        # pMat[iDof,jDof,n] *=  0.5*const_tii * area[n] / he * factor
      # end

    # end
    coef = 0.5*const_tii * area[n] / he * factor

    pMat[2,1,n] = ( nrm1[1,n]*nrm1[1,n] * (GtL[2,1,1,1,n] + GtR[2,1,1,1,n])
                  + nrm1[1,n]*nrm1[2,n] * (GtL[2,1,1,2,n] + GtR[2,1,1,2,n])
                  + nrm1[2,n]*nrm1[1,n] * (GtL[2,1,2,1,n] + GtR[2,1,2,1,n])
                  + nrm1[2,n]*nrm1[2,n] * (GtL[2,1,2,2,n] + GtR[2,1,2,2,n])
                 ) * coef

    pMat[2,2,n] = ( nrm1[1,n]*nrm1[1,n] * (GtL[2,2,1,1,n] + GtR[2,2,1,1,n])
                  + nrm1[2,n]*nrm1[2,n] * (GtL[2,2,2,2,n] + GtR[2,2,2,2,n])
                 ) * coef

    pMat[2,3,n] = ( nrm1[1,n]*nrm1[2,n] * (GtL[2,3,1,2,n] + GtR[2,3,1,2,n])
                  + nrm1[2,n]*nrm1[1,n] * (GtL[2,3,2,1,n] + GtR[2,3,2,1,n])
                 ) * coef

    pMat[3,1,n] = ( nrm1[1,n]*nrm1[1,n] * (GtL[3,1,1,1,n] + GtR[3,1,1,1,n])
                  + nrm1[1,n]*nrm1[2,n] * (GtL[3,1,1,2,n] + GtR[3,1,1,2,n])
                  + nrm1[2,n]*nrm1[1,n] * (GtL[3,1,2,1,n] + GtR[3,1,2,1,n])
                  + nrm1[2,n]*nrm1[2,n] * (GtL[3,1,2,2,n] + GtR[3,1,2,2,n])
                 ) * coef

    pMat[3,2,n] = ( nrm1[1,n]*nrm1[2,n] * (GtL[3,2,1,2,n] + GtR[3,2,1,2,n])
                  + nrm1[2,n]*nrm1[1,n] * (GtL[3,2,2,1,n] + GtR[3,2,2,1,n])
                 ) * coef

    pMat[3,3,n] = ( nrm1[1,n]*nrm1[1,n] * (GtL[3,3,1,1,n] + GtR[3,3,1,1,n])
                  + nrm1[2,n]*nrm1[2,n] * (GtL[3,3,2,2,n] + GtR[3,3,2,2,n])
                 ) * coef

    pMat[4,1,n] = ( nrm1[1,n]*nrm1[1,n] * (GtL[4,1,1,1,n] + GtR[4,1,1,1,n])
                  + nrm1[1,n]*nrm1[2,n] * (GtL[4,1,1,2,n] + GtR[4,1,1,2,n])
                  + nrm1[2,n]*nrm1[1,n] * (GtL[4,1,2,1,n] + GtR[4,1,2,1,n])
                  + nrm1[2,n]*nrm1[2,n] * (GtL[4,1,2,2,n] + GtR[4,1,2,2,n])
                 ) * coef

    pMat[4,2,n] = ( nrm1[1,n]*nrm1[1,n] * (GtL[4,2,1,1,n] + GtR[4,2,1,1,n])
                  + nrm1[1,n]*nrm1[2,n] * (GtL[4,2,1,2,n] + GtR[4,2,1,2,n])
                  + nrm1[2,n]*nrm1[1,n] * (GtL[4,2,2,1,n] + GtR[4,2,2,1,n])
                  + nrm1[2,n]*nrm1[2,n] * (GtL[4,2,2,2,n] + GtR[4,2,2,2,n])
                 ) * coef

    pMat[4,3,n] = ( nrm1[1,n]*nrm1[1,n] * (GtL[4,3,1,1,n] + GtR[4,3,1,1,n])
                  + nrm1[1,n]*nrm1[2,n] * (GtL[4,3,1,2,n] + GtR[4,3,1,2,n])
                  + nrm1[2,n]*nrm1[1,n] * (GtL[4,3,2,1,n] + GtR[4,3,2,1,n])
                  + nrm1[2,n]*nrm1[2,n] * (GtL[4,3,2,2,n] + GtR[4,3,2,2,n])
                 ) * coef

    pMat[4,4,n] = ( nrm1[1,n]*nrm1[1,n] * (GtL[4,4,1,1,n] + GtR[4,4,1,1,n])
                  + nrm1[2,n]*nrm1[2,n] * (GtL[4,4,2,2,n] + GtR[4,4,2,2,n])
                 ) * coef

  end # end of loop over nodes
  return nothing
end

function cmptIPMat_hartman(mesh::AbstractMesh{Tmsh},
                           sbp::AbstractOperator,
                           eqn::NSData{Tsol, Tres, 3},
                           opts,
                           iface::Int,
                           GtL::AbstractArray{Tsol, 5},
                           GtR::AbstractArray{Tsol, 5},
                           pMat::AbstractArray{Tsol, 3}) where {Tmsh, Tsol, Tres}
  # in order to enhance/test stability, we try this factor on penalty terms
  factor = eqn.params.penalty_relaxation
  const_tii = eqn.params.const_tii
  Tdim = 3
  sbpface = mesh.sbpface
  face = mesh.interfaces[iface]
  elemL = face.elementL
  elemR = face.elementR

  # Compute geometric info on face
  nrm1 = Array{Tmsh}(Tdim, mesh.numNodesPerFace)
  area = Array{Tmsh}(mesh.numNodesPerFace)
  for n = 1 : mesh.numNodesPerFace
    nrm_xy = ro_sview(mesh.nrm_face, :, n, iface)
    area[n] = norm(nrm_xy)

    for i = 1 : Tdim
      nrm1[i,n] = nrm_xy[i] / area[n]
    end
  end

  # Compute the size of element and face, and then meas(face)/meas(elem)
  elem_volL = 0.0
  elem_volR = 0.0
  face_area = 0.0
  for n = 1:mesh.numNodesPerElement
    elem_volL +=  sbp.w[n]/mesh.jac[n, elemL]
    elem_volR +=  sbp.w[n]/mesh.jac[n, elemR]
  end

  for n = 1:mesh.numNodesPerFace
    face_area +=  sbpface.wface[n]*area[n]
  end

  heL = elem_volL/eqn.area_sum[elemL]
  heR = elem_volR/eqn.area_sum[elemR]
  he = min(heL, heR)

  for n = 1 : mesh.numNodesPerFace
    # for iDof = 1:mesh.numDofPerNode
      # for jDof = 1:mesh.numDofPerNode
        # pMat[iDof, jDof, n] = 0.0
        # for jDim = 1 : Tdim
          # for iDim = 1 : Tdim
            # pMat[iDof,jDof,n] +=  nrm1[iDim,n]*nrm1[jDim,n] * (GtL[iDof,jDof,iDim,jDim,n] + GtR[iDof,jDof,iDim,jDim,n])
          # end
        # end
        # pMat[iDof,jDof,n] *=  0.5*const_tii * area[n] / he * factor
      # end

    # end
    coef = 0.5*const_tii * area[n] / he * factor

    pMat[2,1,n] = ( nrm1[1,n]*nrm1[1,n] * (GtL[2,1,1,1,n] + GtR[2,1,1,1,n])
                  + nrm1[1,n]*nrm1[2,n] * (GtL[2,1,1,2,n] + GtR[2,1,1,2,n])
                  + nrm1[1,n]*nrm1[3,n] * (GtL[2,1,1,3,n] + GtR[2,1,1,3,n])
                  + nrm1[2,n]*nrm1[1,n] * (GtL[2,1,2,1,n] + GtR[2,1,2,1,n])
                  + nrm1[2,n]*nrm1[2,n] * (GtL[2,1,2,2,n] + GtR[2,1,2,2,n])
                  # + nrm1[2,n]*nrm1[3,n] * (GtL[2,1,2,3,n] + GtR[2,1,2,3,n])
                  + nrm1[3,n]*nrm1[1,n] * (GtL[2,1,3,1,n] + GtR[2,1,3,1,n])
                  # + nrm1[3,n]*nrm1[2,n] * (GtL[2,1,3,2,n] + GtR[2,1,3,2,n])
                  + nrm1[3,n]*nrm1[3,n] * (GtL[2,1,3,3,n] + GtR[2,1,3,3,n])
                 ) * coef

    pMat[2,2,n] = ( nrm1[1,n]*nrm1[1,n] * (GtL[2,2,1,1,n] + GtR[2,2,1,1,n])
                  # + nrm1[1,n]*nrm1[2,n] * (GtL[2,2,1,2,n] + GtR[2,2,1,2,n])
                  # + nrm1[1,n]*nrm1[3,n] * (GtL[2,2,1,3,n] + GtR[2,2,1,3,n])
                  # + nrm1[2,n]*nrm1[1,n] * (GtL[2,2,2,1,n] + GtR[2,2,2,1,n])
                  + nrm1[2,n]*nrm1[2,n] * (GtL[2,2,2,2,n] + GtR[2,2,2,2,n])
                  # + nrm1[2,n]*nrm1[3,n] * (GtL[2,2,2,3,n] + GtR[2,2,2,3,n])
                  # + nrm1[3,n]*nrm1[1,n] * (GtL[2,2,3,1,n] + GtR[2,2,3,1,n])
                  # + nrm1[3,n]*nrm1[2,n] * (GtL[2,2,3,2,n] + GtR[2,2,3,2,n])
                  + nrm1[3,n]*nrm1[3,n] * (GtL[2,2,3,3,n] + GtR[2,2,3,3,n])
                 ) * coef

    pMat[2,3,n] = (
                   # nrm1[1,n]*nrm1[1,n] * (GtL[2,3,1,1,n] + GtR[2,3,1,1,n])
                  + nrm1[1,n]*nrm1[2,n] * (GtL[2,3,1,2,n] + GtR[2,3,1,2,n])
                  # + nrm1[1,n]*nrm1[3,n] * (GtL[2,3,1,3,n] + GtR[2,3,1,3,n])
                  + nrm1[2,n]*nrm1[1,n] * (GtL[2,3,2,1,n] + GtR[2,3,2,1,n])
                  # + nrm1[2,n]*nrm1[2,n] * (GtL[2,3,2,2,n] + GtR[2,3,2,2,n])
                  # + nrm1[2,n]*nrm1[3,n] * (GtL[2,3,2,3,n] + GtR[2,3,2,3,n])
                  # + nrm1[3,n]*nrm1[1,n] * (GtL[2,3,3,1,n] + GtR[2,3,3,1,n])
                  # + nrm1[3,n]*nrm1[2,n] * (GtL[2,3,3,2,n] + GtR[2,3,3,2,n])
                  # + nrm1[3,n]*nrm1[3,n] * (GtL[2,3,3,3,n] + GtR[2,3,3,3,n])
                 ) * coef

    pMat[2,4,n] = (
                    # nrm1[1,n]*nrm1[1,n] * (GtL[2,4,1,1,n] + GtR[2,4,1,1,n])
                  # + nrm1[1,n]*nrm1[2,n] * (GtL[2,4,1,2,n] + GtR[2,4,1,2,n])
                  + nrm1[1,n]*nrm1[3,n] * (GtL[2,4,1,3,n] + GtR[2,4,1,3,n])
                  # + nrm1[2,n]*nrm1[1,n] * (GtL[2,4,2,1,n] + GtR[2,4,2,1,n])
                  # + nrm1[2,n]*nrm1[2,n] * (GtL[2,4,2,2,n] + GtR[2,4,2,2,n])
                  # + nrm1[2,n]*nrm1[3,n] * (GtL[2,4,2,3,n] + GtR[2,4,2,3,n])
                  + nrm1[3,n]*nrm1[1,n] * (GtL[2,4,3,1,n] + GtR[2,4,3,1,n])
                  # + nrm1[3,n]*nrm1[2,n] * (GtL[2,4,3,2,n] + GtR[2,4,3,2,n])
                  # + nrm1[3,n]*nrm1[3,n] * (GtL[2,4,3,3,n] + GtR[2,4,3,3,n])
                 ) * coef

    # pMat[2,5,n] = (
                    # # nrm1[1,n]*nrm1[1,n] * (GtL[2,5,1,1,n] + GtR[2,5,1,1,n])
                  # # + nrm1[1,n]*nrm1[2,n] * (GtL[2,5,1,2,n] + GtR[2,5,1,2,n])
                  # # + nrm1[1,n]*nrm1[3,n] * (GtL[2,5,1,3,n] + GtR[2,5,1,3,n])
                  # # + nrm1[2,n]*nrm1[1,n] * (GtL[2,5,2,1,n] + GtR[2,5,2,1,n])
                  # # + nrm1[2,n]*nrm1[2,n] * (GtL[2,5,2,2,n] + GtR[2,5,2,2,n])
                  # # + nrm1[2,n]*nrm1[3,n] * (GtL[2,5,2,3,n] + GtR[2,5,2,3,n])
                  # # + nrm1[3,n]*nrm1[1,n] * (GtL[2,5,3,1,n] + GtR[2,5,3,1,n])
                  # # + nrm1[3,n]*nrm1[2,n] * (GtL[2,5,3,2,n] + GtR[2,5,3,2,n])
                  # # + nrm1[3,n]*nrm1[3,n] * (GtL[2,5,3,3,n] + GtR[2,5,3,3,n]) )

    pMat[3,1,n] = ( nrm1[1,n]*nrm1[1,n] * (GtL[3,1,1,1,n] + GtR[3,1,1,1,n])
                  + nrm1[1,n]*nrm1[2,n] * (GtL[3,1,1,2,n] + GtR[3,1,1,2,n])
                  # + nrm1[1,n]*nrm1[3,n] * (GtL[3,1,1,3,n] + GtR[3,1,1,3,n])
                  + nrm1[2,n]*nrm1[1,n] * (GtL[3,1,2,1,n] + GtR[3,1,2,1,n])
                  + nrm1[2,n]*nrm1[2,n] * (GtL[3,1,2,2,n] + GtR[3,1,2,2,n])
                  + nrm1[2,n]*nrm1[3,n] * (GtL[3,1,2,3,n] + GtR[3,1,2,3,n])
                  # + nrm1[3,n]*nrm1[1,n] * (GtL[3,1,3,1,n] + GtR[3,1,3,1,n])
                  + nrm1[3,n]*nrm1[2,n] * (GtL[3,1,3,2,n] + GtR[3,1,3,2,n])
                  + nrm1[3,n]*nrm1[3,n] * (GtL[3,1,3,3,n] + GtR[3,1,3,3,n])
                 ) * coef

    pMat[3,2,n] = (
                    # nrm1[1,n]*nrm1[1,n] * (GtL[3,2,1,1,n] + GtR[3,2,1,1,n])
                  + nrm1[1,n]*nrm1[2,n] * (GtL[3,2,1,2,n] + GtR[3,2,1,2,n])
                  # + nrm1[1,n]*nrm1[3,n] * (GtL[3,2,1,3,n] + GtR[3,2,1,3,n])
                  + nrm1[2,n]*nrm1[1,n] * (GtL[3,2,2,1,n] + GtR[3,2,2,1,n])
                  # + nrm1[2,n]*nrm1[2,n] * (GtL[3,2,2,2,n] + GtR[3,2,2,2,n])
                  # + nrm1[2,n]*nrm1[3,n] * (GtL[3,2,2,3,n] + GtR[3,2,2,3,n])
                  # + nrm1[3,n]*nrm1[1,n] * (GtL[3,2,3,1,n] + GtR[3,2,3,1,n])
                  # + nrm1[3,n]*nrm1[2,n] * (GtL[3,2,3,2,n] + GtR[3,2,3,2,n])
                  # + nrm1[3,n]*nrm1[3,n] * (GtL[3,2,3,3,n] + GtR[3,2,3,3,n])
                 ) * coef

    pMat[3,3,n] = ( nrm1[1,n]*nrm1[1,n] * (GtL[3,3,1,1,n] + GtR[3,3,1,1,n])
                  # + nrm1[1,n]*nrm1[2,n] * (GtL[3,3,1,2,n] + GtR[3,3,1,2,n])
                  # + nrm1[1,n]*nrm1[3,n] * (GtL[3,3,1,3,n] + GtR[3,3,1,3,n])
                  # + nrm1[2,n]*nrm1[1,n] * (GtL[3,3,2,1,n] + GtR[3,3,2,1,n])
                  + nrm1[2,n]*nrm1[2,n] * (GtL[3,3,2,2,n] + GtR[3,3,2,2,n])
                  # + nrm1[2,n]*nrm1[3,n] * (GtL[3,3,2,3,n] + GtR[3,3,2,3,n])
                  # + nrm1[3,n]*nrm1[1,n] * (GtL[3,3,3,1,n] + GtR[3,3,3,1,n])
                  # + nrm1[3,n]*nrm1[2,n] * (GtL[3,3,3,2,n] + GtR[3,3,3,2,n])
                  + nrm1[3,n]*nrm1[3,n] * (GtL[3,3,3,3,n] + GtR[3,3,3,3,n])
                 ) * coef

    pMat[3,4,n] = (
                    # nrm1[1,n]*nrm1[1,n] * (GtL[3,4,1,1,n] + GtR[3,4,1,1,n])
                  # + nrm1[1,n]*nrm1[2,n] * (GtL[3,4,1,2,n] + GtR[3,4,1,2,n])
                  # + nrm1[1,n]*nrm1[3,n] * (GtL[3,4,1,3,n] + GtR[3,4,1,3,n])
                  # + nrm1[2,n]*nrm1[1,n] * (GtL[3,4,2,1,n] + GtR[3,4,2,1,n])
                  # + nrm1[2,n]*nrm1[2,n] * (GtL[3,4,2,2,n] + GtR[3,4,2,2,n])
                  + nrm1[2,n]*nrm1[3,n] * (GtL[3,4,2,3,n] + GtR[3,4,2,3,n])
                  # + nrm1[3,n]*nrm1[1,n] * (GtL[3,4,3,1,n] + GtR[3,4,3,1,n])
                  + nrm1[3,n]*nrm1[2,n] * (GtL[3,4,3,2,n] + GtR[3,4,3,2,n])
                  # + nrm1[3,n]*nrm1[3,n] * (GtL[3,4,3,3,n] + GtR[3,4,3,3,n])
                 ) * coef

    # pMat[3,5,n] = (
                    # # nrm1[1,n]*nrm1[1,n] * (GtL[3,5,1,1,n] + GtR[3,5,1,1,n])
                  # # + nrm1[1,n]*nrm1[2,n] * (GtL[3,5,1,2,n] + GtR[3,5,1,2,n])
                  # # + nrm1[1,n]*nrm1[3,n] * (GtL[3,5,1,3,n] + GtR[3,5,1,3,n])
                  # # + nrm1[2,n]*nrm1[1,n] * (GtL[3,5,2,1,n] + GtR[3,5,2,1,n])
                  # # + nrm1[2,n]*nrm1[2,n] * (GtL[3,5,2,2,n] + GtR[3,5,2,2,n])
                  # # + nrm1[2,n]*nrm1[3,n] * (GtL[3,5,2,3,n] + GtR[3,5,2,3,n])
                  # # + nrm1[3,n]*nrm1[1,n] * (GtL[3,5,3,1,n] + GtR[3,5,3,1,n])
                  # # + nrm1[3,n]*nrm1[2,n] * (GtL[3,5,3,2,n] + GtR[3,5,3,2,n])
                  # # + nrm1[3,n]*nrm1[3,n] * (GtL[3,5,3,3,n] + GtR[3,5,3,3,n])
                 # )

    pMat[4,1,n] = ( nrm1[1,n]*nrm1[1,n] * (GtL[4,1,1,1,n] + GtR[4,1,1,1,n])
                  # + nrm1[1,n]*nrm1[2,n] * (GtL[4,1,1,2,n] + GtR[4,1,1,2,n])
                  + nrm1[1,n]*nrm1[3,n] * (GtL[4,1,1,3,n] + GtR[4,1,1,3,n])
                  # + nrm1[2,n]*nrm1[1,n] * (GtL[4,1,2,1,n] + GtR[4,1,2,1,n])
                  + nrm1[2,n]*nrm1[2,n] * (GtL[4,1,2,2,n] + GtR[4,1,2,2,n])
                  + nrm1[2,n]*nrm1[3,n] * (GtL[4,1,2,3,n] + GtR[4,1,2,3,n])
                  + nrm1[3,n]*nrm1[1,n] * (GtL[4,1,3,1,n] + GtR[4,1,3,1,n])
                  + nrm1[3,n]*nrm1[2,n] * (GtL[4,1,3,2,n] + GtR[4,1,3,2,n])
                  + nrm1[3,n]*nrm1[3,n] * (GtL[4,1,3,3,n] + GtR[4,1,3,3,n])
                 ) * coef

    pMat[4,2,n] = (
                    # nrm1[1,n]*nrm1[1,n] * (GtL[4,2,1,1,n] + GtR[4,2,1,1,n])
                  # + nrm1[1,n]*nrm1[2,n] * (GtL[4,2,1,2,n] + GtR[4,2,1,2,n])
                  + nrm1[1,n]*nrm1[3,n] * (GtL[4,2,1,3,n] + GtR[4,2,1,3,n])
                  # + nrm1[2,n]*nrm1[1,n] * (GtL[4,2,2,1,n] + GtR[4,2,2,1,n])
                  # + nrm1[2,n]*nrm1[2,n] * (GtL[4,2,2,2,n] + GtR[4,2,2,2,n])
                  # + nrm1[2,n]*nrm1[3,n] * (GtL[4,2,2,3,n] + GtR[4,2,2,3,n])
                  + nrm1[3,n]*nrm1[1,n] * (GtL[4,2,3,1,n] + GtR[4,2,3,1,n])
                  # + nrm1[3,n]*nrm1[2,n] * (GtL[4,2,3,2,n] + GtR[4,2,3,2,n])
                  # + nrm1[3,n]*nrm1[3,n] * (GtL[4,2,3,3,n] + GtR[4,2,3,3,n])
                 ) * coef

    pMat[4,3,n] = (
                    # nrm1[1,n]*nrm1[1,n] * (GtL[4,3,1,1,n] + GtR[4,3,1,1,n])
                  # + nrm1[1,n]*nrm1[2,n] * (GtL[4,3,1,2,n] + GtR[4,3,1,2,n])
                  # + nrm1[1,n]*nrm1[3,n] * (GtL[4,3,1,3,n] + GtR[4,3,1,3,n])
                  # + nrm1[2,n]*nrm1[1,n] * (GtL[4,3,2,1,n] + GtR[4,3,2,1,n])
                  # + nrm1[2,n]*nrm1[2,n] * (GtL[4,3,2,2,n] + GtR[4,3,2,2,n])
                  + nrm1[2,n]*nrm1[3,n] * (GtL[4,3,2,3,n] + GtR[4,3,2,3,n])
                  # + nrm1[3,n]*nrm1[1,n] * (GtL[4,3,3,1,n] + GtR[4,3,3,1,n])
                  + nrm1[3,n]*nrm1[2,n] * (GtL[4,3,3,2,n] + GtR[4,3,3,2,n])
                  # + nrm1[3,n]*nrm1[3,n] * (GtL[4,3,3,3,n] + GtR[4,3,3,3,n])
                 ) * coef

    pMat[4,4,n] = ( nrm1[1,n]*nrm1[1,n] * (GtL[4,4,1,1,n] + GtR[4,4,1,1,n])
                  # + nrm1[1,n]*nrm1[2,n] * (GtL[4,4,1,2,n] + GtR[4,4,1,2,n])
                  # + nrm1[1,n]*nrm1[3,n] * (GtL[4,4,1,3,n] + GtR[4,4,1,3,n])
                  # + nrm1[2,n]*nrm1[1,n] * (GtL[4,4,2,1,n] + GtR[4,4,2,1,n])
                  + nrm1[2,n]*nrm1[2,n] * (GtL[4,4,2,2,n] + GtR[4,4,2,2,n])
                  # + nrm1[2,n]*nrm1[3,n] * (GtL[4,4,2,3,n] + GtR[4,4,2,3,n])
                  # + nrm1[3,n]*nrm1[1,n] * (GtL[4,4,3,1,n] + GtR[4,4,3,1,n])
                  # + nrm1[3,n]*nrm1[2,n] * (GtL[4,4,3,2,n] + GtR[4,4,3,2,n])
                  + nrm1[3,n]*nrm1[3,n] * (GtL[4,4,3,3,n] + GtR[4,4,3,3,n])
                 ) * coef

    # pMat[4,5,n] = (
                    # # nrm1[1,n]*nrm1[1,n] * (GtL[4,5,1,1,n] + GtR[4,5,1,1,n])
                  # # + nrm1[1,n]*nrm1[2,n] * (GtL[4,5,1,2,n] + GtR[4,5,1,2,n])
                  # # + nrm1[1,n]*nrm1[3,n] * (GtL[4,5,1,3,n] + GtR[4,5,1,3,n])
                  # # + nrm1[2,n]*nrm1[1,n] * (GtL[4,5,2,1,n] + GtR[4,5,2,1,n])
                  # # + nrm1[2,n]*nrm1[2,n] * (GtL[4,5,2,2,n] + GtR[4,5,2,2,n])
                  # # + nrm1[2,n]*nrm1[3,n] * (GtL[4,5,2,3,n] + GtR[4,5,2,3,n])
                  # # + nrm1[3,n]*nrm1[1,n] * (GtL[4,5,3,1,n] + GtR[4,5,3,1,n])
                  # # + nrm1[3,n]*nrm1[2,n] * (GtL[4,5,3,2,n] + GtR[4,5,3,2,n])
                  # # + nrm1[3,n]*nrm1[3,n] * (GtL[4,5,3,3,n] + GtR[4,5,3,3,n])
                 # )

    pMat[5,1,n] = ( nrm1[1,n]*nrm1[1,n] * (GtL[5,1,1,1,n] + GtR[5,1,1,1,n])
                  + nrm1[1,n]*nrm1[2,n] * (GtL[5,1,1,2,n] + GtR[5,1,1,2,n])
                  + nrm1[1,n]*nrm1[3,n] * (GtL[5,1,1,3,n] + GtR[5,1,1,3,n])
                  + nrm1[2,n]*nrm1[1,n] * (GtL[5,1,2,1,n] + GtR[5,1,2,1,n])
                  + nrm1[2,n]*nrm1[2,n] * (GtL[5,1,2,2,n] + GtR[5,1,2,2,n])
                  + nrm1[2,n]*nrm1[3,n] * (GtL[5,1,2,3,n] + GtR[5,1,2,3,n])
                  + nrm1[3,n]*nrm1[1,n] * (GtL[5,1,3,1,n] + GtR[5,1,3,1,n])
                  + nrm1[3,n]*nrm1[2,n] * (GtL[5,1,3,2,n] + GtR[5,1,3,2,n])
                  + nrm1[3,n]*nrm1[3,n] * (GtL[5,1,3,3,n] + GtR[5,1,3,3,n])
                 ) * coef

    pMat[5,2,n] = ( nrm1[1,n]*nrm1[1,n] * (GtL[5,2,1,1,n] + GtR[5,2,1,1,n])
                  + nrm1[1,n]*nrm1[2,n] * (GtL[5,2,1,2,n] + GtR[5,2,1,2,n])
                  + nrm1[1,n]*nrm1[3,n] * (GtL[5,2,1,3,n] + GtR[5,2,1,3,n])
                  + nrm1[2,n]*nrm1[1,n] * (GtL[5,2,2,1,n] + GtR[5,2,2,1,n])
                  + nrm1[2,n]*nrm1[2,n] * (GtL[5,2,2,2,n] + GtR[5,2,2,2,n])
                  # + nrm1[2,n]*nrm1[3,n] * (GtL[5,2,2,3,n] + GtR[5,2,2,3,n])
                  + nrm1[3,n]*nrm1[1,n] * (GtL[5,2,3,1,n] + GtR[5,2,3,1,n])
                  # + nrm1[3,n]*nrm1[2,n] * (GtL[5,2,3,2,n] + GtR[5,2,3,2,n])
                  + nrm1[3,n]*nrm1[3,n] * (GtL[5,2,3,3,n] + GtR[5,2,3,3,n])
                 ) * coef

    pMat[5,3,n] = ( nrm1[1,n]*nrm1[1,n] * (GtL[5,3,1,1,n] + GtR[5,3,1,1,n])
                  + nrm1[1,n]*nrm1[2,n] * (GtL[5,3,1,2,n] + GtR[5,3,1,2,n])
                  # + nrm1[1,n]*nrm1[3,n] * (GtL[5,3,1,3,n] + GtR[5,3,1,3,n])
                  + nrm1[2,n]*nrm1[1,n] * (GtL[5,3,2,1,n] + GtR[5,3,2,1,n])
                  + nrm1[2,n]*nrm1[2,n] * (GtL[5,3,2,2,n] + GtR[5,3,2,2,n])
                  + nrm1[2,n]*nrm1[3,n] * (GtL[5,3,2,3,n] + GtR[5,3,2,3,n])
                  # + nrm1[3,n]*nrm1[1,n] * (GtL[5,3,3,1,n] + GtR[5,3,3,1,n])
                  + nrm1[3,n]*nrm1[2,n] * (GtL[5,3,3,2,n] + GtR[5,3,3,2,n])
                  + nrm1[3,n]*nrm1[3,n] * (GtL[5,3,3,3,n] + GtR[5,3,3,3,n])
                 ) * coef

    pMat[5,4,n] = ( nrm1[1,n]*nrm1[1,n] * (GtL[5,4,1,1,n] + GtR[5,4,1,1,n])
                  # + nrm1[1,n]*nrm1[2,n] * (GtL[5,4,1,2,n] + GtR[5,4,1,2,n])
                  + nrm1[1,n]*nrm1[3,n] * (GtL[5,4,1,3,n] + GtR[5,4,1,3,n])
                  # + nrm1[2,n]*nrm1[1,n] * (GtL[5,4,2,1,n] + GtR[5,4,2,1,n])
                  + nrm1[2,n]*nrm1[2,n] * (GtL[5,4,2,2,n] + GtR[5,4,2,2,n])
                  + nrm1[2,n]*nrm1[3,n] * (GtL[5,4,2,3,n] + GtR[5,4,2,3,n])
                  + nrm1[3,n]*nrm1[1,n] * (GtL[5,4,3,1,n] + GtR[5,4,3,1,n])
                  + nrm1[3,n]*nrm1[2,n] * (GtL[5,4,3,2,n] + GtR[5,4,3,2,n])
                  + nrm1[3,n]*nrm1[3,n] * (GtL[5,4,3,3,n] + GtR[5,4,3,3,n])
                 ) * coef

    pMat[5,5,n] = ( nrm1[1,n]*nrm1[1,n] * (GtL[5,5,1,1,n] + GtR[5,5,1,1,n])
                  # + nrm1[1,n]*nrm1[2,n] * (GtL[5,5,1,2,n] + GtR[5,5,1,2,n])
                  # + nrm1[1,n]*nrm1[3,n] * (GtL[5,5,1,3,n] + GtR[5,5,1,3,n])
                  # + nrm1[2,n]*nrm1[1,n] * (GtL[5,5,2,1,n] + GtR[5,5,2,1,n])
                  + nrm1[2,n]*nrm1[2,n] * (GtL[5,5,2,2,n] + GtR[5,5,2,2,n])
                  # + nrm1[2,n]*nrm1[3,n] * (GtL[5,5,2,3,n] + GtR[5,5,2,3,n])
                  # + nrm1[3,n]*nrm1[1,n] * (GtL[5,5,3,1,n] + GtR[5,5,3,1,n])
                  # + nrm1[3,n]*nrm1[2,n] * (GtL[5,5,3,2,n] + GtR[5,5,3,2,n])
                  + nrm1[3,n]*nrm1[3,n] * (GtL[5,5,3,3,n] + GtR[5,5,3,3,n])
                 ) * coef

  end # end of loop over nodes
  return nothing
end

"""
  compute the interior penalty matrix
  **Input**
   * mesh
   * sbp
   * eqn
   * opts
   * iface: the index of interface
  **Input/Output**
   * pMat: penalty matrix
"""
function cmptBPMat(mesh::AbstractMesh{Tmsh},
                   sbp::AbstractOperator,
                   eqn::NSData{Tsol, Tres, Tdim},
                   opts,
                   iface::Int,
                   Gt::AbstractArray{Tsol, 5},
                   pMat::AbstractArray{Tsol, 3}) where {Tmsh, Tdim, Tsol, Tres}
  if opts["SAT_type"] == "Hartman"
    cmptBPMat_hartman(mesh, sbp, eqn, opts, iface, Gt, pMat)
  elseif opts["SAT_type"] == "SAT-SIPG"
    cmptBPMat_SIPG(mesh, sbp, eqn, opts, iface, Gt, pMat)
  elseif opts["SAT_type"] == "SAT-BR2"
    cmptBPMat_BR2(mesh, sbp, eqn, opts, iface, Gt, pMat)
  end
end

function cmptBPMat_SIPG(mesh::AbstractMesh{Tmsh},
                        sbp::AbstractOperator,
                        eqn::NSData{Tsol, Tres, 2},
                        opts,
                        iface::Int,
                        Gt::AbstractArray{Tsol, 5},
                        pMat::AbstractArray{Tsol, 3}) where {Tmsh, Tsol, Tres}
  error("SAT-SIPG not available yet")
end

function cmptBPMat_BR2(mesh::AbstractMesh{Tmsh},
                       sbp::AbstractOperator,
                       eqn::NSData{Tsol, Tres, 2},
                       opts,
                       iface::Int,
                       Gt::AbstractArray{Tsol, 5},
                       pMat::AbstractArray{Tsol, 3}) where {Tmsh, Tsol, Tres}
  error("SAT-BR2 not available yet")
end

function cmptBPMat_hartman(mesh::AbstractMesh{Tmsh},
                           sbp::AbstractOperator,
                           eqn::NSData{Tsol, Tres, 2},
                           opts,
                           iface::Int,
                           Gt::AbstractArray{Tsol, 5},
                           pMat::AbstractArray{Tsol, 3}) where {Tmsh, Tsol, Tres}
# Reference:
#    Shahbazi, Mavriplis, Multigrid algorithms for high order discontinuous
#    Galerkin discretization of the compressible Navier-Stokes equations,
#    JCP, volume 228 Issue 21, November, 2009
#
  # in order to enhance/test stability, we try this factor on penalty terms
  factor = eqn.params.penalty_relaxation
  const_tii = eqn.params.const_tii
  Tdim = 2
  sbpface = mesh.sbpface

  face = mesh.bndryfaces[iface]
  elem = face.element

  # Compute geometric info on face
  nrm1 = zeros(Tmsh, Tdim, mesh.numNodesPerFace)
  area = zeros(Tmsh, mesh.numNodesPerFace)
  for n = 1 : mesh.numNodesPerFace
    nrm_xy = ro_sview(mesh.nrm_bndry, :, n, iface)
    area[n] = norm(nrm_xy)

    for i = 1 : Tdim
      nrm1[i,n] = nrm_xy[i] / area[n]
    end
  end

  # Compute the size of element and face, and then meas(face)/meas(elem)
  elem_vol = 0.0
  face_area = 0.0
  for n = 1:mesh.numNodesPerElement
    elem_vol +=  sbp.w[n]/mesh.jac[n, elem]
  end

  for n = 1:mesh.numNodesPerFace
    face_area +=  sbpface.wface[n]*area[n]
  end

  he = elem_vol/eqn.area_sum[elem]

  for n = 1 : mesh.numNodesPerFace
    # for iDof = 1:mesh.numDofPerNode
      # for jDof = 1:mesh.numDofPerNode
        # pMat[iDof, jDof, n] = 0.0
        # for jDim = 1 : Tdim
          # for iDim = 1 : Tdim
            # pMat[iDof,jDof,n] +=  nrm1[iDim,n]*nrm1[jDim,n] * (GtL[iDof,jDof,iDim,jDim,n] + GtR[iDof,jDof,iDim,jDim,n])
          # end
        # end
        # pMat[iDof,jDof,n] *=  0.5*const_tii * area[n] / he * factor
      # end

    # end
    coef = const_tii * area[n] / he * factor

    pMat[2,1,n] = ( nrm1[1,n]*nrm1[1,n] * Gt[2,1,1,1,n]
                  + nrm1[1,n]*nrm1[2,n] * Gt[2,1,1,2,n]
                  + nrm1[2,n]*nrm1[1,n] * Gt[2,1,2,1,n]
                  + nrm1[2,n]*nrm1[2,n] * Gt[2,1,2,2,n]
                 ) * coef

    pMat[2,2,n] = ( nrm1[1,n]*nrm1[1,n] * Gt[2,2,1,1,n]
                  + nrm1[2,n]*nrm1[2,n] * Gt[2,2,2,2,n]
                 ) * coef

    pMat[2,3,n] = ( nrm1[1,n]*nrm1[2,n] * Gt[2,3,1,2,n]
                  + nrm1[2,n]*nrm1[1,n] * Gt[2,3,2,1,n]
                 ) * coef

    pMat[3,1,n] = ( nrm1[1,n]*nrm1[1,n] * Gt[3,1,1,1,n]
                  + nrm1[1,n]*nrm1[2,n] * Gt[3,1,1,2,n]
                  + nrm1[2,n]*nrm1[1,n] * Gt[3,1,2,1,n]
                  + nrm1[2,n]*nrm1[2,n] * Gt[3,1,2,2,n]
                 ) * coef

    pMat[3,2,n] = ( nrm1[1,n]*nrm1[2,n] * Gt[3,2,1,2,n]
                  + nrm1[2,n]*nrm1[1,n] * Gt[3,2,2,1,n]
                 ) * coef

    pMat[3,3,n] = ( nrm1[1,n]*nrm1[1,n] * Gt[3,3,1,1,n]
                  + nrm1[2,n]*nrm1[2,n] * Gt[3,3,2,2,n]
                 ) * coef

    pMat[4,1,n] = ( nrm1[1,n]*nrm1[1,n] * Gt[4,1,1,1,n]
                  + nrm1[1,n]*nrm1[2,n] * Gt[4,1,1,2,n]
                  + nrm1[2,n]*nrm1[1,n] * Gt[4,1,2,1,n]
                  + nrm1[2,n]*nrm1[2,n] * Gt[4,1,2,2,n]
                 ) * coef

    pMat[4,2,n] = ( nrm1[1,n]*nrm1[1,n] * Gt[4,2,1,1,n]
                  + nrm1[1,n]*nrm1[2,n] * Gt[4,2,1,2,n]
                  + nrm1[2,n]*nrm1[1,n] * Gt[4,2,2,1,n]
                  + nrm1[2,n]*nrm1[2,n] * Gt[4,2,2,2,n]
                 ) * coef

    pMat[4,3,n] = ( nrm1[1,n]*nrm1[1,n] * Gt[4,3,1,1,n]
                  + nrm1[1,n]*nrm1[2,n] * Gt[4,3,1,2,n]
                  + nrm1[2,n]*nrm1[1,n] * Gt[4,3,2,1,n]
                  + nrm1[2,n]*nrm1[2,n] * Gt[4,3,2,2,n]
                 ) * coef

    pMat[4,4,n] = ( nrm1[1,n]*nrm1[1,n] * Gt[4,4,1,1,n]
                  + nrm1[2,n]*nrm1[1,n] * Gt[4,4,2,1,n]
                  + nrm1[2,n]*nrm1[2,n] * Gt[4,4,2,2,n]
                 ) * coef

  end # end of loop over nodes
  return nothing
end

function cmptBPMat_hartman(mesh::AbstractMesh{Tmsh},
                           sbp::AbstractOperator,
                           eqn::NSData{Tsol, Tres, 3},
                           opts,
                           iface::Int,
                           Gt::AbstractArray{Tsol, 5},
                           pMat::AbstractArray{Tsol, 3}) where {Tmsh, Tsol, Tres}
  # in order to enhance/test stability, we try this factor on penalty terms
  factor = eqn.params.penalty_relaxation
  const_tii = eqn.params.const_tii
  Tdim = 3
  sbpface = mesh.sbpface
  face = mesh.bndryfaces[iface]
  elem = face.element

  # Compute geometric info on face
  nrm1 = Array{Tmsh}(Tdim, mesh.numNodesPerFace)
  area = Array{Tmsh}(mesh.numNodesPerFace)
  for n = 1 : mesh.numNodesPerFace
    nrm_xy = ro_sview(mesh.nrm_bndry, :, n, iface)
    area[n] = norm(nrm_xy)

    for i = 1 : Tdim
      nrm1[i,n] = nrm_xy[i] / area[n]
    end
  end

  # Compute the size of element and face, and then meas(face)/meas(elem)
  elem_vol = 0.0
  face_area = 0.0
  for n = 1:mesh.numNodesPerElement
    elem_vol +=  sbp.w[n]/mesh.jac[n, elem]
  end

  for n = 1:mesh.numNodesPerFace
    face_area +=  sbpface.wface[n]*area[n]
  end

  he = elem_vol/eqn.area_sum[elem]

  for n = 1 : mesh.numNodesPerFace
    # for iDof = 1:mesh.numDofPerNode
      # for jDof = 1:mesh.numDofPerNode
        # pMat[iDof, jDof, n] = 0.0
        # for jDim = 1 : Tdim
          # for iDim = 1 : Tdim
            # pMat[iDof,jDof,n] +=  nrm1[iDim,n]*nrm1[jDim,n] * (GtL[iDof,jDof,iDim,jDim,n] + GtR[iDof,jDof,iDim,jDim,n])
          # end
        # end
        # pMat[iDof,jDof,n] *=  0.5*const_tii * area[n] / he * factor
      # end

    # end
    coef = const_tii * area[n] / he * factor

    pMat[2,1,n] = ( nrm1[1,n]*nrm1[1,n] * Gt[2,1,1,1,n]
                  + nrm1[1,n]*nrm1[2,n] * Gt[2,1,1,2,n]
                  + nrm1[1,n]*nrm1[3,n] * Gt[2,1,1,3,n]
                  + nrm1[2,n]*nrm1[1,n] * Gt[2,1,2,1,n]
                  + nrm1[2,n]*nrm1[2,n] * Gt[2,1,2,2,n]
                  # + nrm1[2,n]*nrm1[3,n] * Gt[2,1,2,3,n]
                  + nrm1[3,n]*nrm1[1,n] * Gt[2,1,3,1,n]
                  # + nrm1[3,n]*nrm1[2,n] * Gt[2,1,3,2,n]
                  + nrm1[3,n]*nrm1[3,n] * Gt[2,1,3,3,n]
                 ) * coef

    pMat[2,2,n] = ( nrm1[1,n]*nrm1[1,n] * Gt[2,2,1,1,n]
                  # + nrm1[1,n]*nrm1[2,n] * Gt[2,2,1,2,n]
                  # + nrm1[1,n]*nrm1[3,n] * Gt[2,2,1,3,n]
                  # + nrm1[2,n]*nrm1[1,n] * Gt[2,2,2,1,n]
                  + nrm1[2,n]*nrm1[2,n] * Gt[2,2,2,2,n]
                  # + nrm1[2,n]*nrm1[3,n] * Gt[2,2,2,3,n]
                  # + nrm1[3,n]*nrm1[1,n] * Gt[2,2,3,1,n]
                  # + nrm1[3,n]*nrm1[2,n] * Gt[2,2,3,2,n]
                  + nrm1[3,n]*nrm1[3,n] * Gt[2,2,3,3,n]
                 ) * coef

    pMat[2,3,n] = (
                   # nrm1[1,n]*nrm1[1,n] * Gt[2,3,1,1,n]
                  + nrm1[1,n]*nrm1[2,n] * Gt[2,3,1,2,n]
                  # + nrm1[1,n]*nrm1[3,n] * Gt[2,3,1,3,n]
                  + nrm1[2,n]*nrm1[1,n] * Gt[2,3,2,1,n]
                  # + nrm1[2,n]*nrm1[2,n] * Gt[2,3,2,2,n]
                  # + nrm1[2,n]*nrm1[3,n] * Gt[2,3,2,3,n]
                  # + nrm1[3,n]*nrm1[1,n] * Gt[2,3,3,1,n]
                  # + nrm1[3,n]*nrm1[2,n] * Gt[2,3,3,2,n]
                  # + nrm1[3,n]*nrm1[3,n] * Gt[2,3,3,3,n]
                 ) * coef

    pMat[2,4,n] = (
                    # nrm1[1,n]*nrm1[1,n] * Gt[2,4,1,1,n]
                  # + nrm1[1,n]*nrm1[2,n] * Gt[2,4,1,2,n]
                  + nrm1[1,n]*nrm1[3,n] * Gt[2,4,1,3,n]
                  # + nrm1[2,n]*nrm1[1,n] * Gt[2,4,2,1,n]
                  # + nrm1[2,n]*nrm1[2,n] * Gt[2,4,2,2,n]
                  # + nrm1[2,n]*nrm1[3,n] * Gt[2,4,2,3,n]
                  + nrm1[3,n]*nrm1[1,n] * Gt[2,4,3,1,n]
                  # + nrm1[3,n]*nrm1[2,n] * Gt[2,4,3,2,n]
                  # + nrm1[3,n]*nrm1[3,n] * Gt[2,4,3,3,n]
                 ) * coef

    # pMat[2,5,n] = (
                    # # nrm1[1,n]*nrm1[1,n] * Gt[2,5,1,1,n]
                  # # + nrm1[1,n]*nrm1[2,n] * Gt[2,5,1,2,n]
                  # # + nrm1[1,n]*nrm1[3,n] * Gt[2,5,1,3,n]
                  # # + nrm1[2,n]*nrm1[1,n] * Gt[2,5,2,1,n]
                  # # + nrm1[2,n]*nrm1[2,n] * Gt[2,5,2,2,n]
                  # # + nrm1[2,n]*nrm1[3,n] * Gt[2,5,2,3,n]
                  # # + nrm1[3,n]*nrm1[1,n] * Gt[2,5,3,1,n]
                  # # + nrm1[3,n]*nrm1[2,n] * Gt[2,5,3,2,n]
                  # # + nrm1[3,n]*nrm1[3,n] * Gt[2,5,3,3,n] )

    pMat[3,1,n] = ( nrm1[1,n]*nrm1[1,n] * Gt[3,1,1,1,n]
                  + nrm1[1,n]*nrm1[2,n] * Gt[3,1,1,2,n]
                  # + nrm1[1,n]*nrm1[3,n] * Gt[3,1,1,3,n]
                  + nrm1[2,n]*nrm1[1,n] * Gt[3,1,2,1,n]
                  + nrm1[2,n]*nrm1[2,n] * Gt[3,1,2,2,n]
                  + nrm1[2,n]*nrm1[3,n] * Gt[3,1,2,3,n]
                  # + nrm1[3,n]*nrm1[1,n] * Gt[3,1,3,1,n]
                  + nrm1[3,n]*nrm1[2,n] * Gt[3,1,3,2,n]
                  + nrm1[3,n]*nrm1[3,n] * Gt[3,1,3,3,n]
                 ) * coef

    pMat[3,2,n] = ( # nrm1[1,n]*nrm1[1,n] * Gt[3,2,1,1,n]
                  + nrm1[1,n]*nrm1[2,n] * Gt[3,2,1,2,n]
                  # + nrm1[1,n]*nrm1[3,n] * Gt[3,2,1,3,n]
                  + nrm1[2,n]*nrm1[1,n] * Gt[3,2,2,1,n]
                  # + nrm1[2,n]*nrm1[2,n] * Gt[3,2,2,2,n]
                  # + nrm1[2,n]*nrm1[3,n] * Gt[3,2,2,3,n]
                  # + nrm1[3,n]*nrm1[1,n] * Gt[3,2,3,1,n]
                  # + nrm1[3,n]*nrm1[2,n] * Gt[3,2,3,2,n]
                  # + nrm1[3,n]*nrm1[3,n] * Gt[3,2,3,3,n]
                 ) * coef

    pMat[3,3,n] = ( nrm1[1,n]*nrm1[1,n] * Gt[3,3,1,1,n]
                  # + nrm1[1,n]*nrm1[2,n] * Gt[3,3,1,2,n]
                  # + nrm1[1,n]*nrm1[3,n] * Gt[3,3,1,3,n]
                  # + nrm1[2,n]*nrm1[1,n] * Gt[3,3,2,1,n]
                  + nrm1[2,n]*nrm1[2,n] * Gt[3,3,2,2,n]
                  # + nrm1[2,n]*nrm1[3,n] * Gt[3,3,2,3,n]
                  # + nrm1[3,n]*nrm1[1,n] * Gt[3,3,3,1,n]
                  # + nrm1[3,n]*nrm1[2,n] * Gt[3,3,3,2,n]
                  + nrm1[3,n]*nrm1[3,n] * Gt[3,3,3,3,n]
                 ) * coef

    pMat[3,4,n] = ( # nrm1[1,n]*nrm1[1,n] * Gt[3,4,1,1,n]
                  # + nrm1[1,n]*nrm1[2,n] * Gt[3,4,1,2,n]
                  # + nrm1[1,n]*nrm1[3,n] * Gt[3,4,1,3,n]
                  # + nrm1[2,n]*nrm1[1,n] * Gt[3,4,2,1,n]
                  # + nrm1[2,n]*nrm1[2,n] * Gt[3,4,2,2,n]
                  + nrm1[2,n]*nrm1[3,n] * Gt[3,4,2,3,n]
                  # + nrm1[3,n]*nrm1[1,n] * Gt[3,4,3,1,n]
                  + nrm1[3,n]*nrm1[2,n] * Gt[3,4,3,2,n]
                  # + nrm1[3,n]*nrm1[3,n] * Gt[3,4,3,3,n]
                 ) * coef

    # pMat[3,5,n] = (
                    # # nrm1[1,n]*nrm1[1,n] * Gt[3,5,1,1,n]
                  # # + nrm1[1,n]*nrm1[2,n] * Gt[3,5,1,2,n]
                  # # + nrm1[1,n]*nrm1[3,n] * Gt[3,5,1,3,n]
                  # # + nrm1[2,n]*nrm1[1,n] * Gt[3,5,2,1,n)
                  # # + nrm1[2,n]*nrm1[2,n] * Gt[3,5,2,2,n]
                  # # + nrm1[2,n]*nrm1[3,n] * Gt[3,5,2,3,n]
                  # # + nrm1[3,n]*nrm1[1,n] * Gt[3,5,3,1,n]
                  # # + nrm1[3,n]*nrm1[2,n] * Gt[3,5,3,2,n]
                  # # + nrm1[3,n]*nrm1[3,n] * Gt[3,5,3,3,n]
                 # )

    pMat[4,1,n] = ( nrm1[1,n]*nrm1[1,n] * Gt[4,1,1,1,n]
                  # + nrm1[1,n]*nrm1[2,n] * Gt[4,1,1,2,n]
                  + nrm1[1,n]*nrm1[3,n] * Gt[4,1,1,3,n]
                  # + nrm1[2,n]*nrm1[1,n] * Gt[4,1,2,1,n]
                  + nrm1[2,n]*nrm1[2,n] * Gt[4,1,2,2,n]
                  + nrm1[2,n]*nrm1[3,n] * Gt[4,1,2,3,n]
                  + nrm1[3,n]*nrm1[1,n] * Gt[4,1,3,1,n]
                  + nrm1[3,n]*nrm1[2,n] * Gt[4,1,3,2,n]
                  + nrm1[3,n]*nrm1[3,n] * Gt[4,1,3,3,n]
                 ) * coef

    pMat[4,2,n] = (
                    # nrm1[1,n]*nrm1[1,n] * Gt[4,2,1,1,n]
                  # + nrm1[1,n]*nrm1[2,n] * Gt[4,2,1,2,n]
                  + nrm1[1,n]*nrm1[3,n] * Gt[4,2,1,3,n]
                  # + nrm1[2,n]*nrm1[1,n] * Gt[4,2,2,1,n]
                  # + nrm1[2,n]*nrm1[2,n] * Gt[4,2,2,2,n]
                  # + nrm1[2,n]*nrm1[3,n] * Gt[4,2,2,3,n]
                  + nrm1[3,n]*nrm1[1,n] * Gt[4,2,3,1,n]
                  # + nrm1[3,n]*nrm1[2,n] * Gt[4,2,3,2,n]
                  # + nrm1[3,n]*nrm1[3,n] * Gt[4,2,3,3,n]
                 ) * coef

    pMat[4,3,n] = (
                    # nrm1[1,n]*nrm1[1,n] * Gt[4,3,1,1,n]
                  # + nrm1[1,n]*nrm1[2,n] * Gt[4,3,1,2,n]
                  # + nrm1[1,n]*nrm1[3,n] * Gt[4,3,1,3,n]
                  # + nrm1[2,n]*nrm1[1,n] * Gt[4,3,2,1,n]
                  # + nrm1[2,n]*nrm1[2,n] * Gt[4,3,2,2,n]
                  + nrm1[2,n]*nrm1[3,n] * Gt[4,3,2,3,n]
                  # + nrm1[3,n]*nrm1[1,n] * Gt[4,3,3,1,n]
                  + nrm1[3,n]*nrm1[2,n] * Gt[4,3,3,2,n]
                  # + nrm1[3,n]*nrm1[3,n] * Gt[4,3,3,3,n]
                 ) * coef

    pMat[4,4,n] = ( nrm1[1,n]*nrm1[1,n] * Gt[4,4,1,1,n]
                  # + nrm1[1,n]*nrm1[2,n] * Gt[4,4,1,2,n]
                  # + nrm1[1,n]*nrm1[3,n] * Gt[4,4,1,3,n]
                  # + nrm1[2,n]*nrm1[1,n] * Gt[4,4,2,1,n]
                  + nrm1[2,n]*nrm1[2,n] * Gt[4,4,2,2,n]
                  # + nrm1[2,n]*nrm1[3,n] * Gt[4,4,2,3,n]
                  # + nrm1[3,n]*nrm1[1,n] * Gt[4,4,3,1,n]
                  # + nrm1[3,n]*nrm1[2,n] * Gt[4,4,3,2,n]
                  + nrm1[3,n]*nrm1[3,n] * Gt[4,4,3,3,n]
                 ) * coef

    # pMat[4,5,n] = (
                    # # nrm1[1,n]*nrm1[1,n] * Gt[4,5,1,1,n]
                  # # + nrm1[1,n]*nrm1[2,n] * Gt[4,5,1,2,n]
                  # # + nrm1[1,n]*nrm1[3,n] * Gt[4,5,1,3,n)
                  # # + nrm1[2,n]*nrm1[1,n] * Gt[4,5,2,1,n]
                  # # + nrm1[2,n]*nrm1[2,n] * Gt[4,5,2,2,n]
                  # # + nrm1[2,n]*nrm1[3,n] * Gt[4,5,2,3,n]
                  # # + nrm1[3,n]*nrm1[1,n] * Gt[4,5,3,1,n]
                  # # + nrm1[3,n]*nrm1[2,n] * Gt[4,5,3,2,n]
                  # # + nrm1[3,n]*nrm1[3,n] * Gt[4,5,3,3,n]
                 # )

    pMat[5,1,n] = ( nrm1[1,n]*nrm1[1,n] * Gt[5,1,1,1,n]
                  + nrm1[1,n]*nrm1[2,n] * Gt[5,1,1,2,n]
                  + nrm1[1,n]*nrm1[3,n] * Gt[5,1,1,3,n]
                  + nrm1[2,n]*nrm1[1,n] * Gt[5,1,2,1,n]
                  + nrm1[2,n]*nrm1[2,n] * Gt[5,1,2,2,n]
                  + nrm1[2,n]*nrm1[3,n] * Gt[5,1,2,3,n]
                  + nrm1[3,n]*nrm1[1,n] * Gt[5,1,3,1,n]
                  + nrm1[3,n]*nrm1[2,n] * Gt[5,1,3,2,n]
                  + nrm1[3,n]*nrm1[3,n] * Gt[5,1,3,3,n]
                 ) * coef

    pMat[5,2,n] = ( nrm1[1,n]*nrm1[1,n] * Gt[5,2,1,1,n]
                  + nrm1[1,n]*nrm1[2,n] * Gt[5,2,1,2,n]
                  + nrm1[1,n]*nrm1[3,n] * Gt[5,2,1,3,n]
                  + nrm1[2,n]*nrm1[1,n] * Gt[5,2,2,1,n]
                  + nrm1[2,n]*nrm1[2,n] * Gt[5,2,2,2,n]
                  # + nrm1[2,n]*nrm1[3,n] * Gt[5,2,2,3,n]
                  + nrm1[3,n]*nrm1[1,n] * Gt[5,2,3,1,n]
                  # + nrm1[3,n]*nrm1[2,n] * Gt[5,2,3,2,n]
                  + nrm1[3,n]*nrm1[3,n] * Gt[5,2,3,3,n]
                 ) * coef

    pMat[5,3,n] = ( nrm1[1,n]*nrm1[1,n] * Gt[5,3,1,1,n]
                  + nrm1[1,n]*nrm1[2,n] * Gt[5,3,1,2,n]
                  # + nrm1[1,n]*nrm1[3,n] * Gt[5,3,1,3,n]
                  + nrm1[2,n]*nrm1[1,n] * Gt[5,3,2,1,n]
                  + nrm1[2,n]*nrm1[2,n] * Gt[5,3,2,2,n]
                  + nrm1[2,n]*nrm1[3,n] * Gt[5,3,2,3,n]
                  # + nrm1[3,n]*nrm1[1,n] * Gt[5,3,3,1,n]
                  + nrm1[3,n]*nrm1[2,n] * Gt[5,3,3,2,n]
                  + nrm1[3,n]*nrm1[3,n] * Gt[5,3,3,3,n]
                 ) * coef

    pMat[5,4,n] = ( nrm1[1,n]*nrm1[1,n] * Gt[5,4,1,1,n]
                  # + nrm1[1,n]*nrm1[2,n] * Gt[5,4,1,2,n]
                  + nrm1[1,n]*nrm1[3,n] * Gt[5,4,1,3,n]
                  # + nrm1[2,n]*nrm1[1,n] * Gt[5,4,2,1,n]
                  + nrm1[2,n]*nrm1[2,n] * Gt[5,4,2,2,n]
                  + nrm1[2,n]*nrm1[3,n] * Gt[5,4,2,3,n]
                  + nrm1[3,n]*nrm1[1,n] * Gt[5,4,3,1,n]
                  + nrm1[3,n]*nrm1[2,n] * Gt[5,4,3,2,n]
                  + nrm1[3,n]*nrm1[3,n] * Gt[5,4,3,3,n]
                 ) * coef

    pMat[5,5,n] = ( nrm1[1,n]*nrm1[1,n] * Gt[5,5,1,1,n]
                  # + nrm1[1,n]*nrm1[2,n] * Gt[5,5,1,2,n]
                  # + nrm1[1,n]*nrm1[3,n] * Gt[5,5,1,3,n]
                  # + nrm1[2,n]*nrm1[1,n] * Gt[5,5,2,1,n]
                  + nrm1[2,n]*nrm1[2,n] * Gt[5,5,2,2,n]
                  # + nrm1[2,n]*nrm1[3,n] * Gt[5,5,2,3,n]
                  # + nrm1[3,n]*nrm1[1,n] * Gt[5,5,3,1,n]
                  # + nrm1[3,n]*nrm1[2,n] * Gt[5,5,3,2,n]
                  + nrm1[3,n]*nrm1[3,n] * Gt[5,5,3,3,n]
                 ) * coef

  end # end of loop over nodes
  return nothing
end


text:
"""
    ccd2rgb(red::ImageHDU, green::ImageHDU, blue::ImageHDU; stretch = identity, shape_out = size(red))
    ccd2rgb(red::Tuple{AbstractMatrix, WCSTransform}, green::Tuple{AbstractMatrix, WCSTransform},
                     blue::Tuple{AbstractMatrix, WCSTransform}; stretch = identity, shape_out = size(red[1]))

Converts 3 grayscale ImageHDU into RGB by reprojecting them.

# Arguments
- `red`: Red channel data.
- `green`: Green channel data.
- `blue`: Blue channel data.
- `stretch`: Stretch function applied.
- `shape_out`: Shape of output RGB image.

# Examples
```julia-repl
julia> ccd2rgb(r, b, g, shape_out = (1000,1000))

julia> ccd2rgb(r, b, g, shape_out = (1000,1000), stretch = log)

julia> ccd2rgb(r, b, g, shape_out = (1000,1000), stretch = sqrt)

julia> ccd2rgb(r, b, g, shape_out = (1000,1000), stretch = asinh)
```
"""
function ccd2rgb(red::Tuple{AbstractMatrix, WCSTransform}, green::Tuple{AbstractMatrix, WCSTransform},
                     blue::Tuple{AbstractMatrix, WCSTransform}; stretch = identity, shape_out = size(red[1]))
    red_rp = reproject(red, red[2], shape_out = shape_out)[1]
    green_rp = reproject(green, red[2], shape_out = shape_out)[1]
    blue_rp = reproject(blue, red[2], shape_out = shape_out)[1]

    I = (red_rp .+ green_rp .+ blue_rp) ./ 3
    I .= (x -> stretch(x)/x).(I)

    red_rp .*= I
    green_rp .*= I
    blue_rp .*= I

    m1 = maximum(x->isnan(x) ? -Inf : x, red_rp)
    m2 = maximum(x->isnan(x) ? -Inf : x, green_rp)
    m3 = maximum(x->isnan(x) ? -Inf : x, blue_rp)
    return colorview(RGB, red_rp./m1 , green_rp./m2, blue_rp./m3)
end

ccd2rgb(red::ImageHDU, green::ImageHDU, blue::ImageHDU; stretch = identity, shape_out = size(red)) =
    ccd2rgb((read(red), WCS.from_header(read_header(red, String))[1]), (read(green), WCS.from_header(read_header(green, String))[1]),
            (read(blue), WCS.from_header(read_header(blue, String))[1]), stretch = stretch, shape_out = shape_out)

ext: jl


text:
# The implementation of caches is influenced by the SparseDiffTools.jl. We implement this
# custom code since the structure of the system models is not compatible with the functionalities
# in SparseDiffTools

struct JacobianFunctionWrapper{F} <: Function
    Jf::F
    Jv::SparseArrays.SparseMatrixCSC{Float64, Int64}
    x::Vector{Float64}
end

function JacobianFunctionWrapper(
    m!::SystemModel{MassMatrixModel},
    x0_guess::Vector{Float64};
    # Improve the heuristic to do sparsity detection
    sparse_retrieve_loop = 3,
)
    x0 = deepcopy(x0_guess)
    n = length(x0)
    m_ = (residual, x) -> m!(residual, x, nothing, 0.0)
    jconfig = ForwardDiff.JacobianConfig(m_, similar(x0), x0, ForwardDiff.Chunk(x0))
    Jf = (Jv, x) -> begin
        @debug "Evaluating Jacobian Function"
        ForwardDiff.jacobian!(Jv, m_, zeros(n), x, jconfig)
        return
    end
    jac = zeros(n, n)
    for _ in 1:sparse_retrieve_loop
        temp = zeros(n, n)
        Jf(temp, abs.(x0 + Random.rand(n) - Random.rand(n)))
        jac .+= abs.(temp)
    end
    Jv = SparseArrays.sparse(jac)
    Jf(Jv, x0)
    return JacobianFunctionWrapper{typeof(Jf)}(Jf, Jv, x0)
end

function JacobianFunctionWrapper(
    m!::SystemModel{ResidualModel},
    x0::Vector{Float64};
    # Improve the heuristic to do sparsity detection
    sparse_retrieve_loop = 3,
)
    n = length(x0)
    m_ = (residual, x) -> m!(residual, zeros(n), x, nothing, 0.0)
    jconfig = ForwardDiff.JacobianConfig(m_, similar(x0), x0, ForwardDiff.Chunk(x0))
    Jf = (Jv, x) -> begin
        @debug "Evaluating Jacobian Function"
        ForwardDiff.jacobian!(Jv, m_, zeros(n), x, jconfig)
        return
    end
    jac = zeros(n, n)
    for _ in 1:sparse_retrieve_loop
        temp = zeros(n, n)
        Jf(temp, abs.(x0 + Random.rand(n) - Random.rand(n)))
        jac .+= abs.(temp)
    end
    Jv = SparseArrays.sparse(jac)
    Jf(Jv, x0)
    return JacobianFunctionWrapper{typeof(Jf)}(Jf, Jv, x0)
end

function (J::JacobianFunctionWrapper)(x::AbstractVector{Float64})
    J.x .= x
    return J.Jf(J.Jv, x)
end

function (J::JacobianFunctionWrapper)(
    JM::SparseArrays.SparseMatrixCSC{Float64, Int64},
    x::AbstractVector{Float64},
)
    J.x .= x
    J.Jf(JM, x)
    return
end

function (J::JacobianFunctionWrapper)(
    JM::SparseArrays.SparseMatrixCSC{Float64, Int64},
    x::AbstractVector{Float64},
    p,
    t,
)
    J.x .= x
    J.Jf(JM, x)
    return
end

function (J::JacobianFunctionWrapper)(
    JM::SparseArrays.SparseMatrixCSC{Float64, Int64},
    dx::AbstractVector{Float64},
    x::AbstractVector{Float64},
    p,
    gamma,
    t,
)
    J.x .= x
    JM .= gamma * LinearAlgebra.Diagonal(ones(length(x))) .- J.Jf(JM, x)
    return JM
end


text:
using Test,
      PhyloModels

# An example from "Molecular Evolution: A Statistical Approach" by Ziheng Yang

# Describe a Phylogenetic Tree
tree = Tree()

# Add 9 nodes
addnodes!(tree, 9)

# Connect with branches...
addbranch!(tree, 9, 6, 0.1)
addbranch!(tree, 9, 8, 0.1)
addbranch!(tree, 6, 7, 0.1)
addbranch!(tree, 6, 3, 0.2)
addbranch!(tree, 7, 1, 0.2)
addbranch!(tree, 7, 2, 0.2)
addbranch!(tree, 8, 4, 0.2)
addbranch!(tree, 8, 5, 0.2)

# Specify sequences of leaf nodes
node_data = NodeDNA()
node_data[1] = DNASeq("T")
node_data[2] = DNASeq("C")
node_data[3] = DNASeq("A")
node_data[4] = DNASeq("C")
node_data[5] = DNASeq("C")

# Specify a Nucleic Acid Substitution Model
model = K80(2.0)

# loglikelihood calculation
ll = loglikelihood(tree, model, node_data)

@test ll ≈ -7.5814075725577

# Simulation
node_data = simulate(DNASeq,
                     tree,
                     model,
                     1000)
@test length(node_data[1]) == 1000


text:
@testset "Newton: copy_traj!" begin
	# test copy_traj!
	s = get_simulation("quadruped", "flat_2D_lc", "flat")
	model = s.model
	env = s.env

	H = 59
	h = 0.1
	nq = model.dim.q

	traj0 = ContactImplicitMPC.contact_trajectory(model, env, H, h)
	traj1 = ContactImplicitMPC.contact_trajectory(model, env, H, h)
	traj1.q[1] .+= 10.0

	ContactImplicitMPC.copy_traj!(traj0, traj1, 1)

	@test traj0.q[1] == 10.0 * ones(nq)

	traj0.q[1] .+= 10.0
	@test traj1.q[1] == 10.0 * ones(nq)
end

@testset "Newton: Residual and Jacobian (configurations and forces)" begin
	s = get_simulation("quadruped", "flat_2D_lc", "flat")
	model = s.model
	env = s.env

	H = 2
	h = 0.1
	nq = model.dim.q
	nu = model.dim.u
	nc = model.dim.c
	nb = nc * friction_dim(env)
	nd = nq + nc + nb
	nr = nq + nu + nc + nb #+ nd

	mode = :configurationforce

	# Test Residual
	r0 = ContactImplicitMPC.NewtonResidual(model, env, H, mode = mode)
	@test length(r0.r) == H * (nr + nd)

	# Test views
	r0.q2[1] .= 1.0
	r0.q2[2] .= 2.0
	@test all(r0.r[nu + nc + nb .+ (1:nq)] .== 1.0)
	@test all(r0.r[nr + nu + nc + nb .+ (1:nq)] .== 2.0)

	# Test views
	r0.r[1:nr] .= -1.0
	@test all(r0.u1[1] .== -1.0)
	@test all(r0.γ1[1] .== -1.0)
	@test all(r0.b1[1] .== -1.0)
	@test all(r0.q2[1] .== -1.0)

	# Test Jacobian
	j0 = ContactImplicitMPC.NewtonJacobian(model, env, H, mode = mode)
	@test size(j0.R) == (H * (nr + nd), H * (nr + nd))

	# Test views
	j0.obj_q2[1] .= 1.0
	j0.obj_q2[2] .= 2.0
	@test all(j0.R[nu + nc + nb .+ (1:nq), nu + nc + nb .+ (1:nq)] .== 1.0)
	@test all(j0.R[nr + nu + nc + nb .+ (1:nq), nr + nu + nc + nb .+ (1:nq)] .== 2.0)

	# Test views
	j0.R[1:nr, 1:nr] .= -1.0
	@test all(j0.obj_u1[1] .== -1.0)
	@test all(j0.obj_γ1[1] .== -1.0)
	@test all(j0.obj_b1[1] .== -1.0)
	@test all(j0.obj_q2[1] .== -1.0)
end

@testset "Newton: jacobian! (configurations and forces)" begin
	s = get_simulation("quadruped", "flat_2D_lc", "flat")
	model = s.model
	env = s.env
	s.model.μ_world = 0.5

	ref_traj = deepcopy(ContactImplicitMPC.get_trajectory(s.model, s.env,
		joinpath(module_dir(), "src/dynamics/quadruped/gaits/gait2.jld2"),
		load_type = :split_traj_alt))
	ContactImplicitMPC.update_friction_coefficient!(ref_traj, s.model, s.env)

	κ = 1.0e-4
	ref_traj.κ .= κ
	H = ref_traj.H
	h = 0.1

	nq = model.dim.q
	nu = model.dim.u
	nc = model.dim.c
	nb = nc * friction_dim(env)
	nd = nq + nc + nb
	nr = nq + nu + nc + nb #+ nd

	mode = :configurationforce

	# Test Jacobian!
	obj = TrackingObjective(s.model, s.env, H,
	    q = [Diagonal(1.0 * ones(nq)) for t = 1:H],
	    u = [Diagonal(1.0e-1 * ones(nu)) for t = 1:H],
	    γ = [Diagonal(1.0e-2 * ones(nc)) for t = 1:H],
	    b = [Diagonal(1.0e-3 * ones(nb)) for t = 1:H])

	im_traj0 = ContactImplicitMPC.ImplicitTraj(ref_traj, s, mode = mode)
	core = ContactImplicitMPC.Newton(s, H, h, ref_traj, im_traj0, obj = obj)
	ContactImplicitMPC.initialize_jacobian!(core.jac, obj, ref_traj.H)
	ContactImplicitMPC.update_jacobian!(core.jac, im_traj0, obj, ref_traj.H, core.β)

	# Test symmetry
	@test core.jac.R - core.jac.R' == spzeros(H * (nr + nd), H * (nr + nd))

	# Test obj function terms and regularization terms
	off = 0
	@test all(abs.(diag(Matrix(core.jac.R[off .+ (1:nu), off .+ (1:nu)] .- 1e-1))) .< 1e-8); off += nu
	@test all(abs.(diag(Matrix(core.jac.R[off .+ (1:nc), off .+ (1:nc)] .- 1e-2))) .< 1e-8); off += nc
	@test all(abs.(diag(Matrix(core.jac.R[off .+ (1:nb), off .+ (1:nb)] .- 1e-3))) .< 1e-8); off += nb
	@test all(abs.(diag(Matrix(core.jac.R[off .+ (1:nq), off .+ (1:nq)] .- 1e-0))) .< 1e-8); off += nq

	# Test dynamics terms
	for t = 1:H
	    @test all(core.jac.IV[t] .== -1.0)
	end

	for t = 3:H
	    @test all(core.jac.q0[t-2] .== im_traj0.δq0[t])
	end

	for t = 2:H
	    @test all(core.jac.q1[t-1] .== im_traj0.δq1[t])
	end

	for t = 1:H
	    @test all(core.jac.u1[t] .== im_traj0.δu1[t])
	end
end

@testset "Newton: residual! (configurations and forces)" begin
	s = get_simulation("quadruped", "flat_2D_lc", "flat")
	model = s.model
	env = s.env
	s.model.μ_world = 0.5

	ref_traj = deepcopy(ContactImplicitMPC.get_trajectory(s.model, s.env,
		joinpath(module_dir(), "src/dynamics/quadruped/gaits/gait2.jld2"),
		load_type = :split_traj_alt))
	ContactImplicitMPC.update_friction_coefficient!(ref_traj, s.model, s.env)

	κ = 1.0e-4
	ref_traj.κ .= κ
	H = ref_traj.H
	h = 0.1

	nq = model.dim.q
	nu = model.dim.u
	nc = model.dim.c
	nb = nc * friction_dim(env)
	nd = nq + nc + nb
	nr = nq + nu + nc + nb

	mode = :configurationforce

	# Test Jacobian!
	obj = TrackingObjective(model, env, H,
	    q = [Diagonal(1.0 * ones(nq)) for t = 1:H],
	    u = [Diagonal(1.0 * ones(nu)) for t = 1:H],
	    γ = [Diagonal(1.0e-6 * ones(nc)) for t = 1:H],
	    b = [Diagonal(1.0e-6 * ones(nb)) for t = 1:H])

	im_traj0 = ContactImplicitMPC.ImplicitTraj(ref_traj, s, mode = mode)
	core = ContactImplicitMPC.Newton(s, H, h, ref_traj, im_traj0, obj = obj)
	ContactImplicitMPC.implicit_dynamics!(im_traj0, s, ref_traj)

	# Offset the trajectory and the dual variables to get a residual
	traj1 = deepcopy(ref_traj)

	for t = 1:H
	    core.ν[t] .+= 2.0
	    traj1.θ[t] .+= 0.1
	    traj1.z[t] .+= 0.1
	    traj1.u[t] .+= 0.1
	    traj1.γ[t] .+= 0.1
	    traj1.b[t] .+= 0.1
	end

	for t = 1:H+2
	    traj1.q[t] .+= 0.1
	end

	ContactImplicitMPC.residual!(core.res, core, core.ν, im_traj0, traj1, ref_traj)

	@test norm(core.Δq[1] .- 0.1, Inf) < 1.0e-8
	@test norm(core.Δu[1] .- 0.1, Inf) < 1.0e-8
	@test norm(core.Δγ[1] .- 0.1, Inf) < 1.0e-8
	@test norm(core.Δb[1] .- 0.1, Inf) < 1.0e-8

	@test (norm(core.res.q2[1] .- core.obj.q[1] * core.Δq[1]
		- im_traj0.δq0[1+2]' * core.ν[1+2] - im_traj0.δq1[1+1]' * core.ν[1+1]
		.+ core.ν[1][1], Inf) < 1.0e-8)
	@test (norm(core.res.u1[1] .- core.obj.u[1] * core.Δu[1]
		- im_traj0.δu1[1]' * core.ν[1], Inf) < 1.0e-8)
	@test (norm(core.res.γ1[1] .- core.obj.γ[1] * core.Δγ[1]
		.+ core.ν[1][1], Inf) < 1.0e-8)
	@test (norm(core.res.b1[1] .- core.obj.b[1] * core.Δb[1]
		.+ core.ν[1][1], Inf) < 1.0e-8)
end

@testset "Newton: Residual and Jacobian (configurations)" begin
	s = get_simulation("quadruped", "flat_2D_lc", "flat")
	model = s.model
	env = s.env

	H = 2
	h = 0.1
	nq = model.dim.q
	nu = model.dim.u
	nd = nq
	nr = nq + nu

	mode = :configuration

	# Test Residual
	r0 = ContactImplicitMPC.NewtonResidual(model, env, H, mode = mode)
	@test length(r0.r) == H * (nr + nd)

	# Test views
	r0.q2[1] .= 1.0
	r0.q2[2] .= 2.0
	@test all(r0.r[nu .+ (1:nq)] .== 1.0)
	@test all(r0.r[nr + nu .+ (1:nq)] .== 2.0)

	# Test views
	r0.r[1:nr] .= -1.0
	@test all(r0.q2[1] .== -1.0)
	@test all(r0.u1[1] .== -1.0)

	# Test Jacobian
	j0 = ContactImplicitMPC.NewtonJacobian(model, env, H, mode = mode)
	@test size(j0.R) == (H * (nr + nd), H * (nr + nd))

	# Test views
	j0.obj_q2[1] .= 1.0
	j0.obj_q2[2] .= 2.0
	@test all(j0.R[nu .+ (1:nq), nu .+ (1:nq)] .== 1.0)
	@test all(j0.R[nr + nu .+ (1:nq), nr + nu .+ (1:nq)] .== 2.0)

	# Test views
	j0.R[1:nr, 1:nr] .= -1.0
	@test all(j0.obj_q2[1] .== -1.0)
	@test all(j0.obj_u1[1] .== -1.0)
end

@testset "Newton: jacobian! (configurations)" begin
	s = get_simulation("quadruped", "flat_2D_lc", "flat")
	model = s.model
	env = s.env
	s.model.μ_world = 0.5

	ref_traj = deepcopy(ContactImplicitMPC.get_trajectory(s.model, s.env,
		joinpath(module_dir(), "src/dynamics/quadruped/gaits/gait2.jld2"),
		load_type = :split_traj_alt))
	ContactImplicitMPC.update_friction_coefficient!(ref_traj, s.model, s.env)

	κ = 1.0e-4
	ref_traj.κ .= κ
	H = ref_traj.H
	h = 0.1

	nq = model.dim.q
	nu = model.dim.u
	nd = nq
	nr = nq + nu

	mode = :configuration

	# Test Jacobian!
	obj = TrackingObjective(s.model, s.env, H,
		q = [Diagonal(1.0 * ones(nq)) for t = 1:H],
		u = [Diagonal(1.0e-1 * ones(nu)) for t = 1:H])

	im_traj0 = ContactImplicitMPC.ImplicitTraj(ref_traj, s, mode = mode)
	core = ContactImplicitMPC.Newton(s, H, h, ref_traj, im_traj0, obj = obj)
	ContactImplicitMPC.initialize_jacobian!(core.jac, obj, ref_traj.H)
	ContactImplicitMPC.update_jacobian!(core.jac, im_traj0, obj, ref_traj.H, core.β)

	# Test symmetry
	@test core.jac.R - core.jac.R' == spzeros(H * (nr + nd), H * (nr + nd))

	# Test obj function terms and regularization terms
	off = 0
	@test all(abs.(diag(Matrix(core.jac.R[off .+ (1:nu), off .+ (1:nu)] .- 1e-1))) .< 1e-8); off += nu
	@test all(abs.(diag(Matrix(core.jac.R[off .+ (1:nq), off .+ (1:nq)] .- 1e-0))) .< 1e-8); off += nq

	# Test dynamics terms
	for t = 1:H
		@test all(core.jac.IV[t] .== -1.0)
	end

	for t = 3:H
		@test all(core.jac.q0[t-2] .== im_traj0.δq0[t])
	end

	for t = 2:H
		@test all(core.jac.q1[t-1] .== im_traj0.δq1[t])
	end

	for t = 1:H
		@test all(core.jac.u1[t] .== im_traj0.δu1[t])
	end
end

@testset "Newton: residual! (configurations)" begin
	s = get_simulation("quadruped", "flat_2D_lc", "flat")
	model = s.model
	env = s.env
	s.model.μ_world = 0.5

	ref_traj = deepcopy(ContactImplicitMPC.get_trajectory(s.model, s.env,
		joinpath(module_dir(), "src/dynamics/quadruped/gaits/gait2.jld2"),
		load_type = :split_traj_alt))
	ContactImplicitMPC.update_friction_coefficient!(ref_traj, s.model, s.env)

	κ = 1.0e-4
	ref_traj.κ .= κ
	H = ref_traj.H
	h = 0.1

	nq = model.dim.q
	nu = model.dim.u
	nc = model.dim.c
	nb = nc * friction_dim(env)
	nd = nq
	nr = nq + nu

	mode = :configuration

	# Test Jacobian!
	obj = TrackingObjective(model, env, H,
	    q = [Diagonal(1.0 * ones(nq)) for t = 1:H],
	    u = [Diagonal(1.0 * ones(nu)) for t = 1:H])

	im_traj0 = ContactImplicitMPC.ImplicitTraj(ref_traj, s, mode = mode)
	core = ContactImplicitMPC.Newton(s, H, h, ref_traj, im_traj0, obj = obj)
	ContactImplicitMPC.implicit_dynamics!(im_traj0, s, ref_traj)

	# Offset the trajectory and the dual variables to get a residual
	traj1 = deepcopy(ref_traj)

	for t = 1:H
	    core.ν[t] .+= 2.0
	    traj1.θ[t] .+= 0.1
	    traj1.z[t] .+= 0.1
	    traj1.u[t] .+= 0.1
	end

	for t = 1:H+2
	    traj1.q[t] .+= 0.1
	end

	ContactImplicitMPC.residual!(core.res, core, core.ν, im_traj0, traj1, ref_traj)

	@test norm(core.Δq[1] .- 0.1, Inf) < 1.0e-8
	@test norm(core.Δu[1] .- 0.1, Inf) < 1.0e-8

	@test (norm(core.res.q2[1] .- core.obj.q[1] * core.Δq[1]
		- im_traj0.δq0[1+2]' * core.ν[1+2] - im_traj0.δq1[1+1]' * core.ν[1+1]
		.+ core.ν[1][1], Inf) < 1.0e-8)
	@test (norm(core.res.u1[1] .- core.obj.u[1] * core.Δu[1]
		- im_traj0.δu1[1]' * core.ν[1], Inf) < 1.0e-8)
end

@testset "Newton: newton_solve!" begin

end


text:
#=
Functions to create a raster from Clouds.

reg_grid and reg_grid_index create a link between raster cells and Points in the
Cloud.
rasterize summarizes the Points per cell to one statistical value with and creates
the actual rasters

=#

struct Raster{U<:Integer}
    pointindex::Vector{Vector{U}}
    nrow::Int
    ncol::Int
    bbox::BoundingBox
    cellsize::Float64
    epsg::Nullable{Int}
    npoints::Int # number of unique points in index
    mask::BitArray{2}  # mask == True -> missing
end

function Raster(
    pointindex::Vector{Vector{U}},
    nrow::Int,
    ncol::Int,
    bbox::BoundingBox,
    cellsize::Float64,
    epsg::Nullable{Int},
    npoints::Int
    ) where U <: Integer

    Raster(pointindex,nrow,ncol,bbox,cellsize,epsg, npoints, falses(nrow,ncol))
end

function Raster(
    pointindex::Vector{Vector{U}},
    nrow::Int,
    ncol::Int,
    bbox::BoundingBox,
    cellsize::Float64,
    epsg::Int,
    npoints::Int,
    mask::BitArray{2}
    ) where U <: Integer

    Raster(pointindex,nrow,ncol,bbox,cellsize,Nullable{Int}(epsg), npoints, mask)
end

function Base.show(io::IO, r::Raster)
    println(io, "RasterIndex (nrow=$(r.nrow), ncol=$(r.ncol), cellsize=$(r.cellsize), n unique points=$(r.npoints), epsg=$(r.epsg), bbox=[$(r.bbox)])")
end

# construct coordinates in vector or meshgrid from Raster
epsg(r::Raster) = r.epsg
npoints(r::Raster) = r.npoints # number of unique points in index

# length is number of cells
Base.length(r::Raster) = length(r.pointindex)
Base.size(r::Raster) = (r.nrow, r.ncol)
Base.eachindex(r::Raster) = eachindex(r.pointindex)

# ind2sub & sub2ind for raster
Base.ind2sub(r::Raster, i::Integer) = ind2sub((r.nrow, r.ncol), i)
Base.sub2ind(r::Raster, row::Integer, col::Integer) = sub2ind((r.nrow, r.ncol), row, col)

"transform coordinates to row, col in raster"
function coords2sub(r::Raster, x::AbstractFloat, y::AbstractFloat)
    # cells include top en left boundaries
    col = Int(fld(x - r.bbox.xmin, r.cellsize)+1)
    row = Int(fld(r.bbox.ymax - y, r.cellsize)+1)
    # include points on lower and right bbox boundaries
    if x == r.bbox.xmax
      col = col - 1
    end
    if y == r.bbox.ymin
      row = row - 1
    end
    ((col <= 0) || (row <= 0) || (row > r.nrow) || (col > r.ncol)) && error("coordinates outside raster domain")
    row, col
end

# based on index of raster pointindex
Base.getindex(r::Raster, i::Integer) = r.pointindex[i]
# based on index of raster, return row, col, pointindex
Base.getindex(r::Raster, row::Integer, col::Integer) = r[sub2ind(r, row, col)]
# based on coordinates raster, return pointindex
function Base.getindex(r::Raster, x::AbstractFloat, y::AbstractFloat)
    row, col = coords2sub(r, x, y)
    r[row, col]
end

"transform coordinates to row, col in raster"
function coords2ind(bbox::BoundingBox,
    nrow::Integer,
    ncol::Integer,
    cellsize::Float64,
    x::AbstractFloat,
    y::AbstractFloat)

    # cells include bottom and left boundaries
    col = Int(fld(x - bbox.xmin, cellsize)+1)
    row = Int(fld(bbox.ymax - y, cellsize)+1)
    # include points on lower and right bbox boundaries
    if x == bbox.xmax
      col = col - 1
    end
    if y == bbox.ymin
      row = row - 1
    end
    isinside = !((col <= 0) || (row <= 0) || (row > nrow) || (col > ncol))
    sub2ind((nrow, ncol), row, col), isinside
end

"transform coordinates to row, col in raster"
function coords2ind_overlap(bbox::BoundingBox,
    nrow::Integer,
    ncol::Integer,
    cellsize::Float64,
    x::AbstractFloat,
    y::AbstractFloat;
    overlap = 100.0)

    # cells include bottom and left boundaries
    col = Int(fld(x - bbox.xmin, cellsize)+1)
    row = Int(fld(bbox.ymax - y, cellsize)+1)
    # include points on lower and right bbox boundaries
    # if x == bbox.xmax
    #   col = col - 1
    # end
    # if y == bbox.ymin
    #   row = row - 1
    # end
    isinside = !((col <= 0) || (row <= 0) || (row > nrow) || (col > ncol))
    # return a list of indices

    # this assumes that the overlap is less than half the cell size
    inleft = rem(x - bbox.xmin, cellsize) <= overlap
    inbottom = rem(y - bbox.ymin, cellsize) <= overlap
    inright = rem(x - bbox.xmin, cellsize) >= (cellsize - overlap)
    intop = rem(y - bbox.ymin, cellsize) >= (cellsize - overlap)

    # correct for boundary issues
    if !isinside
        inleft = false
        inbottom = false
        inright = false
        intop = false
    end
    if col == 1
        inleft = false
    end
    if row == nrow
        inbottom = false
    end
    if col == ncol
        inright = false
    end
    if row == 1
        intop = false
    end

    row, col, isinside, inleft, inbottom, inright, intop
end

"transform row, col in raster to center cell x, y coordinates"
function ind2coords(bbox::BoundingBox,
    nrow::Integer,
    ncol::Integer,
    cellsize::Float64,
    icell::Integer)

    row, col = ind2sub((nrow, ncol), icell)
    # cells include bottom and left boundaries
    x = bbox.xmin + (col - 0.5) * cellsize
    y = bbox.ymax - (row - 0.5) * cellsize

    x, y
end

function rastercellcoordinates(r::Raster)
    ncells = r.ncol * r.nrow
    cellcoords = Vector{SVector{2, Float64}}()
    sizehint!(cellcoords, ncells)
    for icell in 1:ncells
        coords = SVector(ind2coords(r.bbox, r.nrow, r.ncol, r.cellsize, icell))
        push!(cellcoords, coords)
    end
    cellcoords
end

"raster definition constructor from Cloud, cellsize and predefined tiling and epsg"
function define_raster(cloud::Cloud, orig_tile::BoundingBox, overlap::Float64, cellsize::Float64;
    snapgrid=cellsize,
    epsg=Nullable{Int}(),
    pointfilter = nothing)
    # determine inner col and rows and its outer col and rows
    ocol = round(Int, abs(orig_tile.xmax - orig_tile.xmin) / cellsize)
    orow = round(Int, abs(orig_tile.ymax - orig_tile.ymin) / cellsize)
    overlapcells = Int(cld(overlap, cellsize))
    ncol = round(Int, ocol + 2 * overlapcells)
    nrow = round(Int, orow + 2 * overlapcells)

    # create mask and mask out overlaps
    mask = trues(nrow, ncol)
    mask[(overlapcells+1):(end-overlapcells), (overlapcells+1):(end-overlapcells)] = false

    # calculate bounding box including overlap
    bbox = BoundingBox(xmin=orig_tile.xmin - overlapcells * cellsize,
                       xmax=orig_tile.xmax + overlapcells * cellsize,
                       ymin=orig_tile.ymin - overlapcells * cellsize,
                       ymax=orig_tile.ymax + overlapcells * cellsize
                       )

    # create grid definition
    xy_idx, npoints = reg_grid_index(cloud, nrow, ncol, bbox, cellsize;
        pointfilter = pointfilter)
    Raster(xy_idx, nrow, ncol, bbox, cellsize, epsg, npoints, mask)
end

"raster definition constructor from Cloud, cellsize and epsg"
function define_raster(cloud::Cloud, cellsize::Float64;
    snapgrid=cellsize,
    epsg=Nullable{Int}(),
    pointfilter = nothing)

    # create grid definition
    nrow, ncol, bbox = reg_grid(cloud, cellsize; snapgrid=snapgrid)
    xy_idx, npoints = reg_grid_index(cloud, nrow, ncol, bbox, cellsize;
        pointfilter = pointfilter)
    Raster(xy_idx, nrow, ncol, bbox, cellsize, epsg, npoints)
end

"raster definition constructor from Cloud, and GDAL geotransform"
function define_raster(cloud::Cloud, nrow::Integer, ncol::Integer, geotransform::Vector{Float64};
    epsg=Nullable{Int}(),
    pointfilter = nothing)

    # create grid definition
    cellsize = Float64(geotransform[2])
    nrow, ncol, bbox = reg_grid(nrow, ncol, geotransform)
    xy_idx, npoints = reg_grid_index(cloud, nrow, ncol, bbox, cellsize;
        pointfilter = pointfilter)
    Raster(xy_idx, nrow, ncol, bbox, cellsize, epsg, npoints)
end

"raster definition constructor from Cloud, and GDAL geotransform"
function define_raster_overlap(cloud::Cloud, nrow::Integer, ncol::Integer, geotransform::Vector{Float64};
    epsg=Nullable{Int}(),
    pointfilter = nothing,
    overlap = 100.0)

    # create grid definition
    cellsize = Float64(geotransform[2])
    nrow, ncol, bbox = reg_grid(nrow, ncol, geotransform)
    xy_idx, npoints = reg_grid_index_overlap(cloud, nrow, ncol, bbox, cellsize;
        pointfilter = pointfilter,
        overlap = overlap)
    Raster(xy_idx, nrow, ncol, bbox, cellsize, epsg, npoints)
end

"create vectors with bbox from Cloud spatial index based on cell size
snap to utm coordinates by default"
function reg_grid(cloud::Cloud, cellsize::Float64; snapgrid=cellsize)
    # snap bbox to cellsize coordinates (to the outside)
    # this assumes the coordinates are in cm precision
    # do the fld with scaled integers to prevent floating point issues
    # like fld(549.0, 0.01) == 54899.0
    if snapgrid != 0
        snapgrid_int = round(Int32, 100 * snapgrid)
        x_min_int = round(Int32, 100 * boundingbox(cloud).xmin)
        y_min_int = round(Int32, 100 * boundingbox(cloud).ymin)
        x_max_int = round(Int32, 100 * boundingbox(cloud).xmax)
        y_max_int = round(Int32, 100 * boundingbox(cloud).ymax)
        x_min = fld(x_min_int, snapgrid_int) * snapgrid_int * 0.01
        y_min = fld(y_min_int, snapgrid_int) * snapgrid_int * 0.01
        x_max = cld(x_max_int, snapgrid_int) * snapgrid_int * 0.01
        y_max = cld(y_max_int, snapgrid_int) * snapgrid_int * 0.01
    end

    # determine size of grid
    dy = y_max - y_min
    dx = x_max - x_min
    nrow = round(Int, dy / cellsize)
    ncol = round(Int, dx / cellsize)
    bbox = BoundingBox(xmin=x_min, ymin=y_min, xmax=x_max, ymax=y_max, zmin=NaN, zmax=NaN)
    nrow, ncol, bbox
end

"create vectors with bbox based on a GDAL geotransform vector"
function reg_grid(nrow::Integer, ncol::Integer, geotransform::Vector{Float64})
    # assumes a square grid
    cellsize = Float64(geotransform[2])
    x_min = geotransform[1]
    y_max = geotransform[4]
    x_max = x_min + nrow * cellsize
    y_min = y_max - ncol * cellsize
    bbox = BoundingBox(xmin=x_min, ymin=y_min, xmax=x_max, ymax=y_max)

    # determine size of grid
    dy = y_max - y_min
    dx = x_max - x_min
    nrow = round(Int, dy / cellsize)
    ncol = round(Int, dx / cellsize)

    nrow, ncol, bbox
end

"Create reg_grid_index from Cloud and vectors of x & y coordinates"
function reg_grid_index(
    cloud::Cloud,
    nrow::Int,
    ncol::Int,
    bbox::BoundingBox,
    cellsize::Float64;
    pointfilter=nothing)

    n = nrow * ncol # number of cells
    # the idx array gives per LAS point the linear index into the grid it was assigned to
    idx = Array{Vector{Int32}}(n) # some stay zero because of skipped points
    for icell in 1:n
        idx[icell] = Int32[]
    end

    npoints = 0
    @showprogress 2 "Indexing grid..."  for i in 1:length(cloud)
        (pointfilter != nothing) && (pointfilter(cloud, i) || continue)
        # calc cellindex
        pos = positions(cloud)[i]
        xi, yi = pos[1], pos[2]
        icell, isinside = coords2ind(bbox, nrow, ncol, cellsize, xi, yi)
        isinside && push!(idx[icell], i)
        npoints += 1
    end

    idx, npoints
end

"Create reg_grid_index from Cloud and vectors of x & y coordinates"
function reg_grid_index_overlap(
    cloud::Cloud,
    nrow::Int,
    ncol::Int,
    bbox::BoundingBox,
    cellsize::Float64;
    pointfilter = nothing,
    overlap = 100.0)

    n = nrow * ncol # number of cells
    # the idx array gives per LAS point the linear index into the grid it was assigned to
    idx = Array{Vector{Int32}}(n) # some stay zero because of skipped points
    for icell in 1:n
        idx[icell] = Int32[]
    end

    # with overlap the main difference is that each point may not be in 0 or 1 cells,
    # but 0 to n (normally n is max 4)
    npoints = 0
    for i in 1:length(cloud)
        (pointfilter != nothing) && (pointfilter(cloud, i) || continue)
        # calc cellindex
        pos = positions(cloud)[i]
        xi, yi = pos[1], pos[2]
        row, col, isinside, inleft, inbottom, inright, intop = coords2ind_overlap(bbox, nrow, ncol, cellsize, xi, yi, overlap=overlap)
        icell = sub2ind((nrow, ncol), row, col)

        ileft = sub2ind((nrow, ncol), row, col-1)
        ibottom = sub2ind((nrow, ncol), row+1, col)
        iright = sub2ind((nrow, ncol), row, col+1)
        itop = sub2ind((nrow, ncol), row-1, col)

        ileftbottom = sub2ind((nrow, ncol), row+1, col-1)
        irightbottom = sub2ind((nrow, ncol), row+1, col+1)
        irighttop = sub2ind((nrow, ncol), row-1, col+1)
        ilefttop = sub2ind((nrow, ncol), row-1, col-1)

        # center
        isinside && push!(idx[icell], i)
        inleft && push!(idx[ileft], i)
        inbottom && push!(idx[ibottom], i)
        inright && push!(idx[iright], i)
        intop && push!(idx[itop], i)

        # don't only add overlap to the sides but also away from the corners
        inleft && inbottom && push!(idx[ileftbottom], i)
        inright && inbottom && push!(idx[irightbottom], i)
        inright && intop && push!(idx[irighttop], i)
        inleft && intop && push!(idx[ilefttop], i)

        npoints += 1
    end

    idx, npoints
end

"""fill empty raster index cells with nearest neighbor point
this method creates a new raster index"""
function inpaint_missings_nn(r::Raster, cloud::Cloud;
    fill_max_dist = 10)
    # index points that satisfy pointfilter
    index = Int32[]
    for i=1:length(r)
        for idx in r[i]
            push!(index, idx)
        end
    end

    missings = falses((r.nrow, r.ncol))
    idx = deepcopy(r.pointindex)

    # cannot do knn search on empty tree
    if isempty(index)
        return Raster(idx, r.nrow, r.ncol, r.bbox, r.cellsize, r.epsg, r.npoints), trues(r.nrow, r.ncol)
    end

    tree = create_kdtree(cloud; dim=2, index=index)

    if length(tree.data) <= 1
        return r, missings
    end

    @showprogress 1 "Painting missing data..." for icell in 1:length(r)
        isempty(idx[icell]) || continue
        missings[icell] = true
        # for empty cells find nearest neighbor point
        x, y = ind2coords(r.bbox,r.nrow,r.ncol,r.cellsize,icell)
        i, d = knn(tree, [x, y], 1)
        d[1] <= fill_max_dist && push!(idx[icell], index[i[1]])
    end

    # number of unique points unchanged
    Raster(idx, r.nrow, r.ncol, r.bbox, r.cellsize, r.epsg, r.npoints), missings
end

"""filter a raster index with a pointfilter"""
function filter_raster(r::Raster, cloud::Cloud, pointfilter)
    n = length(r)
    # create empty index
    idx = Array{Vector{Int32}}(n) # some stay zero because of skipped points
    for icell in 1:n
        idx[icell] = Int32[]
    end

    # index points that satisfy pointfilter
    npoints = 0
    for icell=1:n
        for i in r[icell]
            (pointfilter != nothing) && (pointfilter(cloud, i) || continue)
            push!(idx[icell], i)
            npoints += 1
        end
    end

    # return new index
    Raster(idx, r.nrow, r.ncol, r.bbox, r.cellsize, r.epsg, npoints)
end

"generic function to create 2d array from Cloud
inputs
cloud            Cloud
vx, vy        x&y coordinate vectors defining the grid
xy_idx        Cloud indices per gridcell
reducer    statistical function to summarize points per cell
      function reducer(cloud::Cloud, index::Vector{Int})
        Vector{Real}
      end
pointfilter function to filter points
      function pointfilter(cloud::Cloud, index::Int)
        true
      end
"
function rasterize(cloud::Cloud, r::Raster;
    reducer = reducer_minz, # function to calculate value per cell pased on
    pointfilter = nothing, # predicate function to filter individual points
    min_dens = 0,
    nodata = -9999.0,
    return_density = false)

    # check dimensions of output
    nlayers = 1 + return_density
    area = r.cellsize^2

    # setup output arrays
    raster = fill(Float32(nodata), r.nrow, r.ncol, nlayers)

    # loop through cells
    for i in 1:length(r)
        # find points in range
        idx0 = r[i]
        row, col = ind2sub(r, i)
        # loop through points and filter
        if pointfilter != nothing # if filter is given
            idx = Int[] #  indices after filter
            for i in idx0
                pointfilter(cloud, i) || continue
                push!(idx, i)
            end
        else
            idx = idx0 # no filter applied
        end

        np = length(idx)
        # calculate density [points / m2]
        di = Float64(np) / area
        if return_density
            raster[row, col, end] = di # save to end layer
        end
        if min_dens > 0
            di < min_dens && continue # min density threshold
        end

        np < 1 && continue # stat functions don't work on empty arrays
        # set statistics to grid
        raster[row, col, 1:end-return_density] = reducer(cloud, idx)
    end

    # return raster with statistics, density is saved to last layer
    raster
end


text:
"""
    show(io::IO, N::AbstractEcologicalNetwork)
"""
function Base.show(io::IO, N::AbstractEcologicalNetwork)
  p = typeof(N) <: AbstractBipartiteNetwork ? "bipartite" : "unipartite"
  t = ""
  t = typeof(N) <: ProbabilisticNetwork ? " probabilistic" : t
  t = typeof(N) <: QuantitativeNetwork ? " quantitative" : t
  print(io, "$(richness(N; dims=1))×$(richness(N; dims=2)) ($(_species_type(N))) $(p)$(t) ecological network (L: $(links(N)) - $(_interaction_type(N)))")
end

"""
Creates a copy of a network -- this returns an object with the same type, and
the same content.
"""
function Base.copy(N::AbstractEcologicalNetwork)
  edges = copy(N.edges)
  sp = copy.(_species_objects(N))
  return typeof(N)(edges, sp...)
end

"""
    Base.:!{T<:DeterministicNetwork}(N::T)

Returns the inverse of a binary network -- interactions that were `false` become
`true`, and conversely.
"""
function Base.:!(N::T) where {T <: BinaryNetwork}
  return typeof(N)(dropzeros(.!N.edges), _species_objects(N)...)
end

"""
    permutedims(N::AbstractBipartiteNetwork)

Tranposes the network and returns a copy
"""
function Base.permutedims(N::AbstractEcologicalNetwork)
  NA = permutedims(N.edges)
  new_sp = typeof(N) <: AbstractBipartiteNetwork ? (N.B, N.T) : (N.S,)
  return typeof(N)(NA, new_sp...)
end

"""
    +(n1::T, n2::T) where {T <: BipartiteQuantitativeNetwork}

Adds two quantitative bipartite networks. TODO
"""
function Base.:+(n1::T, n2::T) where {T <: BipartiteQuantitativeNetwork}
  st = union(species(n1; dims=1), species(n2; dims=1))
  sb = union(species(n1; dims=2), species(n2; dims=2))
  A = zeros(_interaction_type(n1), (length(st), length(sb)))
  N = T(A, st, sb)
  for i1 in n1
    N[i1.from,i1.to] = N[i1.from,i1.to] + i1.strength
  end
  for i2 in n2
    N[i2.from,i2.to] = N[i2.from,i2.to] + i2.strength
  end
  return N
end


text:
# This file is a part of Julia. License is MIT: https://julialang.org/license

function _truncate_at_width_or_chars(str, width, chars="", truncmark="…")
    truncwidth = textwidth(truncmark)
    (width <= 0 || width < truncwidth) && return ""

    wid = truncidx = lastidx = 0
    for (idx, c) in pairs(str)
        lastidx = idx
        wid += textwidth(c)
        wid >= width - truncwidth && truncidx == 0 && (truncidx = lastidx)
        (wid >= width || c in chars) && break
    end

    lastidx != 0 && str[lastidx] in chars && (lastidx = prevind(str, lastidx))
    truncidx == 0 && (truncidx = lastidx)
    if lastidx < lastindex(str)
        return String(SubString(str, 1, truncidx) * truncmark)
    else
        return String(str)
    end
end

function show(io::IO, t::AbstractDict{K,V}) where V where K
    recur_io = IOContext(io, :SHOWN_SET => t,
                             :typeinfo => eltype(t))

    limit = get(io, :limit, false)::Bool
    # show in a Julia-syntax-like form: Dict(k=>v, ...)
    print(io, typeinfo_prefix(io, t)[1])
    print(io, '(')
    if !isempty(t) && !show_circular(io, t)
        first = true
        n = 0
        for pair in t
            first || print(io, ", ")
            first = false
            show(recur_io, pair)
            n+=1
            limit && n >= 10 && (print(io, "…"); break)
        end
    end
    print(io, ')')
end

# Dict

# These can be changed, to trade off better performance for space
const global maxallowedprobe = 16
const global maxprobeshift   = 6

"""
    Dict([itr])

`Dict{K,V}()` constructs a hash table with keys of type `K` and values of type `V`.
Keys are compared with [`isequal`](@ref) and hashed with [`hash`](@ref).

Given a single iterable argument, constructs a [`Dict`](@ref) whose key-value pairs
are taken from 2-tuples `(key,value)` generated by the argument.

# Examples
```jldoctest
julia> Dict([("A", 1), ("B", 2)])
Dict{String, Int64} with 2 entries:
  "B" => 2
  "A" => 1
```

Alternatively, a sequence of pair arguments may be passed.

```jldoctest
julia> Dict("A"=>1, "B"=>2)
Dict{String, Int64} with 2 entries:
  "B" => 2
  "A" => 1
```
"""
mutable struct Dict{K,V} <: AbstractDict{K,V}
    slots::Array{UInt8,1}
    keys::Array{K,1}
    vals::Array{V,1}
    ndel::Int
    count::Int
    age::UInt
    idxfloor::Int  # an index <= the indices of all used slots
    maxprobe::Int

    function Dict{K,V}() where V where K
        n = 16
        new(zeros(UInt8,n), Vector{K}(undef, n), Vector{V}(undef, n), 0, 0, 0, 1, 0)
    end
    function Dict{K,V}(d::Dict{K,V}) where V where K
        new(copy(d.slots), copy(d.keys), copy(d.vals), d.ndel, d.count, d.age,
            d.idxfloor, d.maxprobe)
    end
    function Dict{K, V}(slots, keys, vals, ndel, count, age, idxfloor, maxprobe) where {K, V}
        new(slots, keys, vals, ndel, count, age, idxfloor, maxprobe)
    end
end
function Dict{K,V}(kv) where V where K
    h = Dict{K,V}()
    haslength(kv) && sizehint!(h, Int(length(kv))::Int)
    for (k,v) in kv
        h[k] = v
    end
    return h
end
Dict{K,V}(p::Pair) where {K,V} = setindex!(Dict{K,V}(), p.second, p.first)
function Dict{K,V}(ps::Pair...) where V where K
    h = Dict{K,V}()
    sizehint!(h, length(ps))
    for p in ps
        h[p.first] = p.second
    end
    return h
end
# Note the constructors of WeakKeyDict mirror these here, keep in sync.
Dict() = Dict{Any,Any}()
Dict(kv::Tuple{}) = Dict()
copy(d::Dict) = Dict(d)

const AnyDict = Dict{Any,Any}

Dict(ps::Pair{K,V}...) where {K,V} = Dict{K,V}(ps)
Dict(ps::Pair...)                  = Dict(ps)

function Dict(kv)
    try
        dict_with_eltype((K, V) -> Dict{K, V}, kv, eltype(kv))
    catch
        if !isiterable(typeof(kv)) || !all(x->isa(x,Union{Tuple,Pair}),kv)
            throw(ArgumentError("Dict(kv): kv needs to be an iterator of tuples or pairs"))
        else
            rethrow()
        end
    end
end

function grow_to!(dest::AbstractDict{K, V}, itr) where V where K
    y = iterate(itr)
    y === nothing && return dest
    ((k,v), st) = y
    dest2 = empty(dest, typeof(k), typeof(v))
    dest2[k] = v
    grow_to!(dest2, itr, st)
end

# this is a special case due to (1) allowing both Pairs and Tuples as elements,
# and (2) Pair being invariant. a bit annoying.
function grow_to!(dest::AbstractDict{K,V}, itr, st) where V where K
    y = iterate(itr, st)
    while y !== nothing
        (k,v), st = y
        if isa(k,K) && isa(v,V)
            dest[k] = v
        else
            new = empty(dest, promote_typejoin(K,typeof(k)), promote_typejoin(V,typeof(v)))
            merge!(new, dest)
            new[k] = v
            return grow_to!(new, itr, st)
        end
        y = iterate(itr, st)
    end
    return dest
end

empty(a::AbstractDict, ::Type{K}, ::Type{V}) where {K, V} = Dict{K, V}()

hashindex(key, sz) = (((hash(key)::UInt % Int) & (sz-1)) + 1)::Int

@propagate_inbounds isslotempty(h::Dict, i::Int) = h.slots[i] == 0x0
@propagate_inbounds isslotfilled(h::Dict, i::Int) = h.slots[i] == 0x1
@propagate_inbounds isslotmissing(h::Dict, i::Int) = h.slots[i] == 0x2

function rehash!(h::Dict{K,V}, newsz = length(h.keys)) where V where K
    olds = h.slots
    oldk = h.keys
    oldv = h.vals
    sz = length(olds)
    newsz = _tablesz(newsz)
    h.age += 1
    h.idxfloor = 1
    if h.count == 0
        resize!(h.slots, newsz)
        fill!(h.slots, 0)
        resize!(h.keys, newsz)
        resize!(h.vals, newsz)
        h.ndel = 0
        return h
    end

    slots = zeros(UInt8,newsz)
    keys = Vector{K}(undef, newsz)
    vals = Vector{V}(undef, newsz)
    age0 = h.age
    count = 0
    maxprobe = 0

    for i = 1:sz
        @inbounds if olds[i] == 0x1
            k = oldk[i]
            v = oldv[i]
            index0 = index = hashindex(k, newsz)
            while slots[index] != 0
                index = (index & (newsz-1)) + 1
            end
            probe = (index - index0) & (newsz-1)
            probe > maxprobe && (maxprobe = probe)
            slots[index] = 0x1
            keys[index] = k
            vals[index] = v
            count += 1

            if h.age != age0
                # if `h` is changed by a finalizer, retry
                return rehash!(h, newsz)
            end
        end
    end

    h.slots = slots
    h.keys = keys
    h.vals = vals
    h.count = count
    h.ndel = 0
    h.maxprobe = maxprobe
    @assert h.age == age0

    return h
end

function sizehint!(d::Dict{T}, newsz) where T
    oldsz = length(d.slots)
    # limit new element count to max_values of the key type
    newsz = min(newsz, max_values(T)::Int)
    # need at least 1.5n space to hold n elements
    newsz = cld(3 * newsz, 2)
    if newsz <= oldsz
        # todo: shrink
        # be careful: rehash!() assumes everything fits. it was only designed
        # for growing.
        return d
    end
    rehash!(d, newsz)
end

"""
    empty!(collection) -> collection

Remove all elements from a `collection`.

# Examples
```jldoctest
julia> A = Dict("a" => 1, "b" => 2)
Dict{String, Int64} with 2 entries:
  "b" => 2
  "a" => 1

julia> empty!(A);

julia> A
Dict{String, Int64}()
```
"""
function empty!(h::Dict{K,V}) where V where K
    fill!(h.slots, 0x0)
    sz = length(h.slots)
    empty!(h.keys)
    empty!(h.vals)
    resize!(h.keys, sz)
    resize!(h.vals, sz)
    h.ndel = 0
    h.count = 0
    h.age += 1
    h.idxfloor = 1
    return h
end

# get the index where a key is stored, or -1 if not present
function ht_keyindex(h::Dict{K,V}, key) where V where K
    sz = length(h.keys)
    iter = 0
    maxprobe = h.maxprobe
    index = hashindex(key, sz)
    keys = h.keys

    @inbounds while true
        if isslotempty(h,index)
            break
        end
        if !isslotmissing(h,index) && (key === keys[index] || isequal(key,keys[index]))
            return index
        end

        index = (index & (sz-1)) + 1
        iter += 1
        iter > maxprobe && break
    end
    return -1
end

# get the index where a key is stored, or -pos if not present
# and the key would be inserted at pos
# This version is for use by setindex! and get!
function ht_keyindex2!(h::Dict{K,V}, key) where V where K
    sz = length(h.keys)
    iter = 0
    maxprobe = h.maxprobe
    index = hashindex(key, sz)
    avail = 0
    keys = h.keys

    @inbounds while true
        if isslotempty(h,index)
            if avail < 0
                return avail
            end
            return -index
        end

        if isslotmissing(h,index)
            if avail == 0
                # found an available slot, but need to keep scanning
                # in case "key" already exists in a later collided slot.
                avail = -index
            end
        elseif key === keys[index] || isequal(key, keys[index])
            return index
        end

        index = (index & (sz-1)) + 1
        iter += 1
        iter > maxprobe && break
    end

    avail < 0 && return avail

    maxallowed = max(maxallowedprobe, sz>>maxprobeshift)
    # Check if key is not present, may need to keep searching to find slot
    @inbounds while iter < maxallowed
        if !isslotfilled(h,index)
            h.maxprobe = iter
            return -index
        end
        index = (index & (sz-1)) + 1
        iter += 1
    end

    rehash!(h, h.count > 64000 ? sz*2 : sz*4)

    return ht_keyindex2!(h, key)
end

@propagate_inbounds function _setindex!(h::Dict, v, key, index)
    h.slots[index] = 0x1
    h.keys[index] = key
    h.vals[index] = v
    h.count += 1
    h.age += 1
    if index < h.idxfloor
        h.idxfloor = index
    end

    sz = length(h.keys)
    # Rehash now if necessary
    if h.ndel >= ((3*sz)>>2) || h.count*3 > sz*2
        # > 3/4 deleted or > 2/3 full
        rehash!(h, h.count > 64000 ? h.count*2 : h.count*4)
    end
    nothing
end

function setindex!(h::Dict{K,V}, v0, key0) where V where K
    key = convert(K, key0)
    if !isequal(key, key0)
        throw(ArgumentError("$(limitrepr(key0)) is not a valid key for type $K"))
    end
    setindex!(h, v0, key)
end

function setindex!(h::Dict{K,V}, v0, key::K) where V where K
    v = convert(V, v0)
    index = ht_keyindex2!(h, key)

    if index > 0
        h.age += 1
        @inbounds h.keys[index] = key
        @inbounds h.vals[index] = v
    else
        @inbounds _setindex!(h, v, key, -index)
    end

    return h
end

function setindex!(h::Dict{K,Any}, v, key::K) where K
    @nospecialize v
    index = ht_keyindex2!(h, key)

    if index > 0
        h.age += 1
        @inbounds h.keys[index] = key
        @inbounds h.vals[index] = v
    else
        @inbounds _setindex!(h, v, key, -index)
    end

    return h
end

"""
    get!(collection, key, default)

Return the value stored for the given key, or if no mapping for the key is present, store
`key => default`, and return `default`.

# Examples
```jldoctest
julia> d = Dict("a"=>1, "b"=>2, "c"=>3);

julia> get!(d, "a", 5)
1

julia> get!(d, "d", 4)
4

julia> d
Dict{String, Int64} with 4 entries:
  "c" => 3
  "b" => 2
  "a" => 1
  "d" => 4
```
"""
get!(collection, key, default)

"""
    get!(f::Function, collection, key)

Return the value stored for the given key, or if no mapping for the key is present, store
`key => f()`, and return `f()`.

This is intended to be called using `do` block syntax.

# Examples
```jldoctest
julia> squares = Dict{Int, Int}();

julia> function get_square!(d, i)
           get!(d, i) do
               i^2
           end
       end
get_square! (generic function with 1 method)

julia> get_square!(squares, 2)
4

julia> squares
Dict{Int64, Int64} with 1 entry:
  2 => 4
```
"""
get!(f::Function, collection, key)

function get!(default::Callable, h::Dict{K,V}, key0) where V where K
    key = convert(K, key0)
    if !isequal(key, key0)
        throw(ArgumentError("$(limitrepr(key0)) is not a valid key for type $K"))
    end
    return get!(default, h, key)
end

function get!(default::Callable, h::Dict{K,V}, key::K) where V where K
    index = ht_keyindex2!(h, key)

    index > 0 && return h.vals[index]

    age0 = h.age
    v = convert(V, default())
    if h.age != age0
        index = ht_keyindex2!(h, key)
    end
    if index > 0
        h.age += 1
        @inbounds h.keys[index] = key
        @inbounds h.vals[index] = v
    else
        @inbounds _setindex!(h, v, key, -index)
    end
    return v
end

function getindex(h::Dict{K,V}, key) where V where K
    index = ht_keyindex(h, key)
    @inbounds return (index < 0) ? throw(KeyError(key)) : h.vals[index]::V
end

"""
    get(collection, key, default)

Return the value stored for the given key, or the given default value if no mapping for the
key is present.

!!! compat "Julia 1.7"
    For tuples and numbers, this function requires at least Julia 1.7.

# Examples
```jldoctest
julia> d = Dict("a"=>1, "b"=>2);

julia> get(d, "a", 3)
1

julia> get(d, "c", 3)
3
```
"""
get(collection, key, default)

function get(h::Dict{K,V}, key, default) where V where K
    index = ht_keyindex(h, key)
    @inbounds return (index < 0) ? default : h.vals[index]::V
end

"""
    get(f::Function, collection, key)

Return the value stored for the given key, or if no mapping for the key is present, return
`f()`.  Use [`get!`](@ref) to also store the default value in the dictionary.

This is intended to be called using `do` block syntax

```julia
get(dict, key) do
    # default value calculated here
    time()
end
```
"""
get(::Function, collection, key)

function get(default::Callable, h::Dict{K,V}, key) where V where K
    index = ht_keyindex(h, key)
    @inbounds return (index < 0) ? default() : h.vals[index]::V
end

"""
    haskey(collection, key) -> Bool

Determine whether a collection has a mapping for a given `key`.

# Examples
```jldoctest
julia> D = Dict('a'=>2, 'b'=>3)
Dict{Char, Int64} with 2 entries:
  'a' => 2
  'b' => 3

julia> haskey(D, 'a')
true

julia> haskey(D, 'c')
false
```
"""
haskey(h::Dict, key) = (ht_keyindex(h, key) >= 0)
in(key, v::KeySet{<:Any, <:Dict}) = (ht_keyindex(v.dict, key) >= 0)

"""
    getkey(collection, key, default)

Return the key matching argument `key` if one exists in `collection`, otherwise return `default`.

# Examples
```jldoctest
julia> D = Dict('a'=>2, 'b'=>3)
Dict{Char, Int64} with 2 entries:
  'a' => 2
  'b' => 3

julia> getkey(D, 'a', 1)
'a': ASCII/Unicode U+0061 (category Ll: Letter, lowercase)

julia> getkey(D, 'd', 'a')
'a': ASCII/Unicode U+0061 (category Ll: Letter, lowercase)
```
"""
function getkey(h::Dict{K,V}, key, default) where V where K
    index = ht_keyindex(h, key)
    @inbounds return (index<0) ? default : h.keys[index]::K
end

function _pop!(h::Dict, index)
    @inbounds val = h.vals[index]
    _delete!(h, index)
    return val
end

function pop!(h::Dict, key)
    index = ht_keyindex(h, key)
    return index > 0 ? _pop!(h, index) : throw(KeyError(key))
end

"""
    pop!(collection, key[, default])

Delete and return the mapping for `key` if it exists in `collection`, otherwise return
`default`, or throw an error if `default` is not specified.

# Examples
```jldoctest
julia> d = Dict("a"=>1, "b"=>2, "c"=>3);

julia> pop!(d, "a")
1

julia> pop!(d, "d")
ERROR: KeyError: key "d" not found
Stacktrace:
[...]

julia> pop!(d, "e", 4)
4
```
"""
pop!(collection, key, default)

function pop!(h::Dict, key, default)
    index = ht_keyindex(h, key)
    return index > 0 ? _pop!(h, index) : default
end

function pop!(h::Dict)
    isempty(h) && throw(ArgumentError("dict must be non-empty"))
    idx = skip_deleted_floor!(h)
    @inbounds key = h.keys[idx]
    @inbounds val = h.vals[idx]
    _delete!(h, idx)
    key => val
end

function _delete!(h::Dict{K,V}, index) where {K,V}
    @inbounds h.slots[index] = 0x2
    @inbounds _unsetindex!(h.keys, index)
    @inbounds _unsetindex!(h.vals, index)
    h.ndel += 1
    h.count -= 1
    h.age += 1
    return h
end

"""
    delete!(collection, key)

Delete the mapping for the given key in a collection, if any, and return the collection.

# Examples
```jldoctest
julia> d = Dict("a"=>1, "b"=>2)
Dict{String, Int64} with 2 entries:
  "b" => 2
  "a" => 1

julia> delete!(d, "b")
Dict{String, Int64} with 1 entry:
  "a" => 1

julia> delete!(d, "b") # d is left unchanged
Dict{String, Int64} with 1 entry:
  "a" => 1
```
"""
delete!(collection, key)

function delete!(h::Dict, key)
    index = ht_keyindex(h, key)
    if index > 0
        _delete!(h, index)
    end
    return h
end

function skip_deleted(h::Dict, i)
    L = length(h.slots)
    for i = i:L
        @inbounds if isslotfilled(h,i)
            return  i
        end
    end
    return 0
end
function skip_deleted_floor!(h::Dict)
    idx = skip_deleted(h, h.idxfloor)
    if idx != 0
        h.idxfloor = idx
    end
    idx
end

@propagate_inbounds _iterate(t::Dict{K,V}, i) where {K,V} = i == 0 ? nothing : (Pair{K,V}(t.keys[i],t.vals[i]), i == typemax(Int) ? 0 : i+1)
@propagate_inbounds function iterate(t::Dict)
    _iterate(t, skip_deleted_floor!(t))
end
@propagate_inbounds iterate(t::Dict, i) = _iterate(t, skip_deleted(t, i))

isempty(t::Dict) = (t.count == 0)
length(t::Dict) = t.count

@propagate_inbounds function Base.iterate(v::T, i::Int = v.dict.idxfloor) where T <: Union{KeySet{<:Any, <:Dict}, ValueIterator{<:Dict}}
    i == 0 && return nothing
    i = skip_deleted(v.dict, i)
    i == 0 && return nothing
    vals = T <: KeySet ? v.dict.keys : v.dict.vals
    (@inbounds vals[i], i == typemax(Int) ? 0 : i+1)
end

function filter!(pred, h::Dict{K,V}) where {K,V}
    h.count == 0 && return h
    @inbounds for i=1:length(h.slots)
        if h.slots[i] == 0x01 && !pred(Pair{K,V}(h.keys[i], h.vals[i]))
            _delete!(h, i)
        end
    end
    return h
end

function reduce(::typeof(merge), items::Vector{<:Dict})
    K = mapreduce(keytype, promote_type, items)
    V = mapreduce(valtype, promote_type, items)
    return reduce(merge!, items; init=Dict{K,V}())
end

function map!(f, iter::ValueIterator{<:Dict})
    dict = iter.dict
    vals = dict.vals
    # @inbounds is here so that it gets propagated to isslotfilled
    @inbounds for i = dict.idxfloor:lastindex(vals)
        if isslotfilled(dict, i)
            vals[i] = f(vals[i])
        end
    end
    return iter
end

function mergewith!(combine, d1::Dict{K, V}, d2::AbstractDict) where {K, V}
    for (k, v) in d2
        i = ht_keyindex2!(d1, k)
        if i > 0
            d1.vals[i] = combine(d1.vals[i], v)
        else
            if !isequal(k, convert(K, k))
                throw(ArgumentError("$(limitrepr(k)) is not a valid key for type $K"))
            end
            @inbounds _setindex!(d1, convert(V, v), k, -i)
        end
    end
    return d1
end

struct ImmutableDict{K,V} <: AbstractDict{K,V}
    parent::ImmutableDict{K,V}
    key::K
    value::V
    ImmutableDict{K,V}() where {K,V} = new() # represents an empty dictionary
    ImmutableDict{K,V}(key, value) where {K,V} = (empty = new(); new(empty, key, value))
    ImmutableDict{K,V}(parent::ImmutableDict, key, value) where {K,V} = new(parent, key, value)
end

"""
    ImmutableDict

`ImmutableDict` is a dictionary implemented as an immutable linked list,
which is optimal for small dictionaries that are constructed over many individual insertions.
Note that it is not possible to remove a value, although it can be partially overridden and hidden
by inserting a new value with the same key.

    ImmutableDict(KV::Pair)

Create a new entry in the `ImmutableDict` for a `key => value` pair

 - use `(key => value) in dict` to see if this particular combination is in the properties set
 - use `get(dict, key, default)` to retrieve the most recent value for a particular key

"""
ImmutableDict
ImmutableDict(KV::Pair{K,V}) where {K,V} = ImmutableDict{K,V}(KV[1], KV[2])
ImmutableDict(t::ImmutableDict{K,V}, KV::Pair) where {K,V} = ImmutableDict{K,V}(t, KV[1], KV[2])
ImmutableDict(t::ImmutableDict{K,V}, KV::Pair, rest::Pair...) where {K,V} =
    ImmutableDict(ImmutableDict(t, KV), rest...)
ImmutableDict(KV::Pair, rest::Pair...) = ImmutableDict(ImmutableDict(KV), rest...)

function in(key_value::Pair, dict::ImmutableDict, valcmp=(==))
    key, value = key_value
    while isdefined(dict, :parent)
        if isequal(dict.key, key)
            valcmp(value, dict.value) && return true
        end
        dict = dict.parent
    end
    return false
end

function haskey(dict::ImmutableDict, key)
    while isdefined(dict, :parent)
        isequal(dict.key, key) && return true
        dict = dict.parent
    end
    return false
end

function getindex(dict::ImmutableDict, key)
    while isdefined(dict, :parent)
        isequal(dict.key, key) && return dict.value
        dict = dict.parent
    end
    throw(KeyError(key))
end
function get(dict::ImmutableDict, key, default)
    while isdefined(dict, :parent)
        isequal(dict.key, key) && return dict.value
        dict = dict.parent
    end
    return default
end

function get(default::Callable, dict::ImmutableDict, key)
    while isdefined(dict, :parent)
        isequal(dict.key, key) && return dict.value
        dict = dict.parent
    end
    return default()
end

# this actually defines reverse iteration (e.g. it should not be used for merge/copy/filter type operations)
function iterate(d::ImmutableDict{K,V}, t=d) where {K, V}
    !isdefined(t, :parent) && return nothing
    (Pair{K,V}(t.key, t.value), t.parent)
end
length(t::ImmutableDict) = count(Returns(true), t)
isempty(t::ImmutableDict) = !isdefined(t, :parent)
empty(::ImmutableDict, ::Type{K}, ::Type{V}) where {K, V} = ImmutableDict{K,V}()

_similar_for(c::AbstractDict, ::Type{Pair{K,V}}, itr, isz, len) where {K, V} = empty(c, K, V)
_similar_for(c::AbstractDict, ::Type{T}, itr, isz, len) where {T} =
    throw(ArgumentError("for AbstractDicts, similar requires an element type of Pair;\n  if calling map, consider a comprehension instead"))

max_stars_repo_name: tgflynn/julia

max_issues_repo_name: tgflynn/julia

max_forks_repo_name: tgflynn/julia


text:
# Load Julia packages (libraries) needed.

using StatisticalRethinking

ProjDir = @__DIR__

# Simulate uncorrelated data

m_5_1 = "
data {
 int < lower = 1 > N; // Sample size
 vector[N] D; // Outcome
 vector[N] A; // Predictor
}

parameters {
 real a; // Intercept
 real bA; // Slope (regression coefficients)
 real < lower = 0 > sigma;    // Error SD
}

model {
  vector[N] mu;               // mu is a vector
  a ~ normal(0, 0.2);         //Priors
  bA ~ normal(0, 0.5);
  sigma ~ exponential(1);
  mu = a + bA * A;
  D ~ normal(mu , sigma);     // Likelihood
}
";

# Define the SampleModel and set the output format to :mcmcchains.

m5_1s = SampleModel("m5.1", m_5_1);

# Input data for cmdstan

ad_data = Dict(
  "N" => size(df, 1),
  "D" => df[!, :D],
  "A" => df[!, :A]
);

# Sample using StanSample

rc = stan_sample(m5_1s, data=ad_data);

if success(rc)

  # Describe the draws

  dfa1 = read_samples(m5_1s; output_format=:dataframe)
  Particles(dfa1)

end