"""
    _preduceBF!(M, N, Q, Z, L::Union{AbstractMatrix,Missing}, R::Union{AbstractMatrix,Missing};
                fast = true, atol = 0, rtol, roff = 0, coff = 0, rtrail = 0, ctrail = 0,
                withQ = true, withZ = true) -> n, m, p

Reduce the partitioned matrix pencil `M - λN`

              [   *         *         *  ] roff
     M - λN = [   0      M22-λN22     *  ] npp
              [   0         0         *  ] rtrail
                coff       npm     ctrail

to an equivalent basic form `F - λG = Q1'*(M - λN)*Z1` using orthogonal transformation matrices `Q1` and `Z1`
such that the subpencil `M22 - λN22` is transformed into the following standard form

                       | B  | A-λE |
          F22 - λG22 = |----|------| ,
                       | D  |  C   |

where `E` is an `nxn` non-singular matrix, and `A`, `B`, `C`, `D` are `nxn`-, `nxm`-, `pxn`- and `pxm`-dimensional matrices,
respectively. The order `n` of `E` is equal to the numerical rank of `N` determined using the absolute tolerance `atol` and
relative tolerance `rtol`. `M` and `N` are overwritten by `F` and `G`, respectively.

The performed orthogonal or unitary transformations are accumulated in `Q` (i.e., `Q <- Q*Q1`), if `withQ = true`, and
`Z` (i.e., `Z <- Z*Z1`), if `withZ = true`.

The  matrix `L` is overwritten by `Q1'*L` unless `L = missing` and the  matrix `R` is overwritten by `R*Z1` unless `R = missing`.

If `fast = true`, `E` is determined upper triangular using a rank revealing QR-decomposition with column pivoting of `N22`
and `n` is evaluated as the number of nonzero diagonal elements of the R factor, whose magnitudes are greater than
`tol = max(atol,abs(R[1,1])*rtol)`.
If `fast = false`,  `E` is determined diagonal using a rank revealing SVD-decomposition of `N22` and
`n` is evaluated as the number of singular values greater than `tol = max(atol,smax*rtol)`, where `smax`
is the largest singular value.
The rank decision based on the SVD-decomposition is generally more reliable, but the involved computational effort is higher.
"""
function _preduceBF!(M::AbstractMatrix{T}, N::AbstractMatrix{T},
                     Q::Union{AbstractMatrix{T},Nothing}, Z::Union{AbstractMatrix{T},Nothing},
                     L::Union{AbstractVecOrMat{T},Missing} = missing, R::Union{AbstractMatrix{T},Missing} = missing;
                     atol::Real = zero(real(T)), rtol::Real = (min(size(M)...)*eps(real(float(one(T)))))*iszero(atol),
                     fast::Bool = true, roff::Int = 0, coff::Int = 0, rtrail::Int = 0, ctrail::Int = 0,
                     withQ::Bool = true, withZ::Bool = true) where T <: BlasFloat
   # In interest of performance, no dimensional checks are performed
   mM, nM = size(M)
   npp = mM-rtrail-roff
   npm = nM-ctrail-coff
   # assume the partitioned form
   #
   #              [M11-λN11  M12-λN12  M13-λN13] roff
   #     M - λN = [   0      M22-λN22  M23-λM23] npp
   #              [   0         0      M33-λN33] rtrail
   #                coff       npm      ctrail
   #
   # where M22 and N22 are npp x npm matrices

   # fast return for null dimensions
   (npp == 0 || npm == 0) && (return 0, npm, npp)

   # Step 0: Reduce M22 -λ N22 to the standard form
   i11 = 1:roff
   i22 = roff+1:roff+npp
   j22 = coff+1:coff+npm
   i12 = 1:roff+npp
   j23 = coff+1:nM
   jN23 = coff+npm+1:nM
   T <: Complex ? tran = 'C' : tran = 'T'
   Q === nothing && (withQ = false)
   Z === nothing && (withZ = false)
   if fast
      # compute in-place the QR-decomposition N22*P2 = Q2*[R2;0] with column pivoting
      N22 = view(N,i22,j22)
      _, τ, jpvt = LinearAlgebra.LAPACK.geqp3!(N22)
      tol = max(atol, rtol*abs(N22[1,1]))
      n = count(x -> x > tol, abs.(diag(N22)))
      m = npm-n
      p = npp-n
      i1 = 1:n
      # [M22 M23] <- Q2'*[M22 M23]
      LinearAlgebra.LAPACK.ormqr!('L',tran,N22,τ,view(M,i22,j23))
      # [M12; M22] <- [M12; M22]*P2
      M[i12,j22] = M[i12,j22[jpvt]]
      # N23 <- Q2'*N23
      LinearAlgebra.LAPACK.ormqr!('L',tran,N22,τ,view(N,i22,jN23))
      # N12 <- N12*P2
      N[i11,j22] = N[i11,j22[jpvt]]
      # Q <- Q*Q2
      withQ && LinearAlgebra.LAPACK.ormqr!('R','N',N22,τ,view(Q,:,i22))
      # Z <- Z*P2
      withZ && (Z[:,j22] = Z[:,j22[jpvt]])
      # L <- Q2'*L
      ismissing(L) || LinearAlgebra.LAPACK.ormqr!('L',tran,N22,τ,view(L,i22,:))
      # N22 = [R2;0]
      N22[:,:] = [ triu(N22[i1,:]); zeros(T,p,npm) ]
       # R <- R*P2
      ismissing(R) || (R[:,j22] = R[:,j22[jpvt]])

      # compute in-place the complete orthogonal decomposition N22*Z2*Pc2 = [0 E; 0 0] with E nonsingular and UT
      _, tau = LinearAlgebra.LAPACK.tzrzf!(view(N22,i1,:))
      jp = [coff+n+1:coff+npm; coff+1:coff+n]
      N22r = view(N22,i1,:)
      # [M12; M22] <- [M12; M22]*Z2*Pc2
      LinearAlgebra.LAPACK.ormrz!('R',tran,N22r,tau,view(M,i12,j22))
      M[i12,j22] = M[i12,jp]
      # N12 <- N12*Z2*Pc2
      LinearAlgebra.LAPACK.ormrz!('R',tran,N22r,tau,view(N,i11,j22))
      N[i11,j22] = N[i11,jp]
      if withZ
         LinearAlgebra.LAPACK.ormrz!('R',tran,N22r,tau,view(Z,:,j22))
         Z[:,j22] = Z[:,jp]
      end
      # R <- R*Z2*Pc2
      ismissing(R) || (LinearAlgebra.LAPACK.ormrz!('R',tran,N22r,tau,view(R,:,j22)); R[:,j22] = R[:,jp])
      N22[:,:] = [ zeros(T,n,m) triu(N22[i1,i1]); zeros(T,p,npm) ]
   else
      # compute the complete orthogonal decomposition of N22 using the SVD-decomposition
      U, S, Vt = LinearAlgebra.LAPACK.gesdd!('A',view(N,i22,j22))
      tol = max(atol, rtol*S[1])
      n = count(x -> x > tol, S)
      m = npm-n
      p = npp-n
      jp = [coff+n+1:coff+npm; coff+1:coff+n]
      # [M12; M22] <- [M12; M22]*V*Pc
      M[i12,j22] = M[i12,j22]*Vt'
      M[i12,j22] = M[i12,jp]
      # [M22 M23] <- U'*[M22 M23]
      M[i22,j23] = U'*M[i22,j23]
      # N12 <- N12*V*Pc
      N[i11,j22] = N[i11,j22]*Vt'
      N[i11,j22] = N[i11,jp]
      # N23 <- U'*N23
      N[i22,jN23] = U'*N[i22,jN23]
      # Q <- Q*U
      withQ && (Q[:,i22] = Q[:,i22]*U)
      # Z <- Q*V
      if withZ
         Z[:,j22] = Z[:,j22]*Vt'
         Z[:,j22] = Z[:,jp]
      end
      # L <- U'*L
      ismissing(L) || (L[i22,:] = U'*L[i22,:])
      # R <- R*V
      ismissing(R) || (R[:,j22] = R[:,j22]*Vt'; R[:,j22] = R[:,jp] )
      N[i22,j22] = [ zeros(T,n,m) Diagonal(S[1:n]) ; zeros(T,p,npm) ]
   end
   return n, m, p
end
"""
    _preduce1!(n::Int, m::Int, p::Int, M::AbstractMatrix, N::AbstractMatrix,
               Q::Union{AbstractMatrix,Nothing}, Z::Union{AbstractMatrix,Nothing}, tol,
               L::Union{AbstractMatrix{T},Missing} = missing, R::Union{AbstractMatrix{T},Missing} = missing;
               fast = true, roff = 0, coff = 0, rtrail = 0, ctrail = 0, withQ = true, withZ = true)

Reduce the structured pencil  `M - λN`

     M = [ *  * *  *  ] roff          N = [ *  * *  *  ] roff
         [ 0  B A  *  ] n                 [ 0  0 E  *  ] n
         [ 0  D C  *  ] p                 [ 0  0 0  *  ] p
         [ 0  * *  *  ] rtrail            [ 0  * *  *  ] rtrail
         coff m n ctrail                  coff m n ctrail

with E upper triangular and nonsingular to the following form `M1 - λN1 = Q1'*(M - λN)*Z1` with

    M1 = [ *   *    *    *    *  ] roff       N1 = [ *   *  *   *    *   ] roff
         [ 0   B1   A11 A12   *  ] τ+ρ             [ 0   0  E11 E12  *   ] τ+ρ
         [ 0   0    B2  A22   *  ] n-ρ             [ 0   0  0   E22  *   ] n-ρ
         [ 0   0    D2  C2    *  ] p-τ             [ 0   0  0   0    *   ] p-τ
         [ 0   *    *   *     *  ] rtrail          [ 0   *  *   *    *   ] rtrail
          coff m    ρ  n-ρ ctrail                   coff m  ρ  n-ρ ctrail

where τ = rank D, B1 is full row rank, and E22 is upper triangular and nonsingular.
The performed orthogonal or unitary transformations are accumulated in `Q` (i.e., `Q <- Q*Q1`), if `withQ = true`, and
`Z` (i.e., `Z <- Z*Z1`), if `withZ = true`. The rank decisions use the absolute tolerance `tol` for the nonzero elements of `M`.

The  matrix `L` is overwritten by `Q1'*L` unless `L = missing` and the  matrix `R` is overwritten by `R*Z1` unless `R = missing`.
"""
function _preduce1!(n::Int, m::Int, p::Int, M::AbstractMatrix{T}, N::AbstractMatrix{T},
                    Q::Union{AbstractMatrix{T},Nothing}, Z::Union{AbstractMatrix{T},Nothing}, tol::Real,
                    L::Union{AbstractVecOrMat{T},Missing} = missing, R::Union{AbstractMatrix{T},Missing} = missing;
                    fast::Bool = true, roff::Int = 0, coff::Int = 0, rtrail::Int = 0, ctrail::Int = 0,
                    withQ::Bool = true, withZ::Bool = true) where T <: BlasFloat
   #  Steps 1 and 2: QR- or SVD-decomposition based full column compression of [B; D] with the UT-form preservation of E
   #  Reduce the structured pencil
   #
   #  M = [ *  * *  *  ] roff          N = [ *  * *  *  ] roff
   #      [ 0  B A  *  ] n                 [ 0  0 E  *  ] n
   #      [ 0  D C  *  ] p                 [ 0  0 0  *  ] p
   #      [ 0  * *  *  ] rtrail            [ 0  * *  *  ] rtrail
   #      coff m n ctrail                  coff m n ctrail
   #
   #  with E upper triangular and nonsingular to the following form
   #
   #  M1 = [ *   *    *    *    *  ] roff        N1 = [ *   *  *   *    *   ] roff
   #       [ 0   B1   A11 A12   *  ] τ+ρ              [ 0   0  E11 E12  *   ] τ+ρ
   #       [ 0   0    B2  A22   *  ] n-ρ              [ 0   0  0   E22  *   ] n-ρ
   #       [ 0   0    D2  C2    *  ] p-τ              [ 0   0  0   0    *   ] p-τ
   #       [ 0   *    *   *     *  ] rtrail           [ 0   *  *   *    *   ] rtrail
   #        coff m    ρ  n-ρ ctrail                    coff m  ρ  n-ρ ctrail
   #
   #  where τ = rank D, B1 is full row rank and E22 is upper triangular and nonsingular.

   npm = n+m
   npp = n+p
   ZERO = zero(T)
   mM = roff + npp + rtrail
   nM = coff + npm + ctrail
   # Step 1:
   ia = roff+1:roff+n
   ja = coff+m+1:nM
   jb = coff+1:coff+m
   B = view(M,ia,jb)
   BD = view(M,roff+1:roff+npp,jb)
   if p > 0
      # compress D to [D1 D2;0 0] with D1 invertible
      ic = roff+n+1:roff+npp
      D = view(M,ic,jb)
      CE = view(M,ic,ja)
      EE = view(N,ic,coff+npm+1:nM)
      if fast
         QR = qr!(D, Val(true))
         τ = count(x -> x > tol, abs.(diag(QR.R)))
      else
         τ = count(x -> x > tol, svdvals(D))
         QR = qr!(D, Val(true))
      end
      B[:,:] = B[:,QR.p]
      lmul!(QR.Q',CE)
      lmul!(QR.Q',EE)
      withQ && rmul!(view(Q,:,ic),QR.Q)
      ismissing(L) || lmul!(QR.Q',view(L,ic,:))
      D[:,:] = [ QR.R[1:τ,:]; zeros(T,p-τ,m) ]
      # Step 2:
      k = 1
      for j = coff+1:coff+τ
         for ii = roff+n+k:-1:roff+1+k
            iim1 = ii-1
            G, M[iim1,j] = givens(M[iim1,j],M[ii,j],iim1,ii)
            M[ii,j] = ZERO
            lmul!(G,view(M,:,j+1:nM))
            lmul!(G,view(N,:,ja))  # more efficient computation possible by selecting only the relevant columns
            withQ && rmul!(Q,G')
            ismissing(L) || lmul!(G,L)
         end
         k += 1
      end
   else
      τ = 0
   end
   ρ = _preduce3!(n, m-τ, M, N, Q, Z, tol, L, R, fast = fast, coff = coff+τ, roff = roff+τ, rtrail = rtrail, ctrail = ctrail, withQ = withQ, withZ = withZ)
   if p > 0
      irt = 1:(τ+ρ)
      BD[irt,:] = BD[irt,invperm(QR.p)]
   end
   return τ, ρ
end
"""
    _preduce2!(n::Int, m::Int, p::Int, M::AbstractMatrix, N::AbstractMatrix,
               Q::Union{AbstractMatrix,Nothing}, Z::Union{AbstractMatrix,Nothing}, tol,
               L::Union{AbstractMatrix{T},Missing} = missing, R::Union{AbstractMatrix{T},Missing} = missing;
               fast = true, roff = 0, coff = 0, rtrail = 0, ctrail = 0, withQ = true, withZ = true)

Reduce the structured pencil

     M = [ *  * *   *  ] roff          N = [ *  * *  *   ] roff
         [ *  B A   *  ] n                 [ 0  0 E  *   ] n
         [ *  D C   *  ] p                 [ 0  0 0  *   ] p
         [ 0  0 0   *  ] rtrail            [ 0  0 0  *   ] rtrail
         coff m n ctrail                   coff m n ctrail

with `E` upper triangular and nonsingular to the following form `M1 - λN1 = Q1'*(M - λN)*Z1` with

    M1 = [ *    *   *    *    *  ] roff       N1 = [ *    *   *   *    *   ] roff
         [ *    B1  A11 A12   *  ] n-ρ             [ 0    0  E11 E12   *   ] n-ρ
         [ *    D1  C1  A22   *  ] ρ               [ 0    0   0  E22   *   ] ρ
         [ 0    0   0   C2    *  ] p               [ 0    0   0   0    *   ] p
         [ 0    0   0   0     *  ] rtrail          [ 0    0   0   0    *   ] rtrail
          coff m-τ n-ρ τ+ρ ctrail                   coff m-τ n-ρ τ+ρ ctrail

where `τ = rank D`, `C2` is full column rank and `E11` and `E22` are upper triangular and nonsingular.
The performed orthogonal or unitary transformations are accumulated in `Q` (i.e., `Q <- Q*Q1`), if `withQ = true`, and
`Z` (i.e., `Z <- Z*Z1`), if `withZ = true`. The rank decisions use the absolute tolerance `tol` for the nonzero elements of `M`.

The  matrix `L` is overwritten by `Q1'*L` unless `L = missing` and the  matrix `R` is overwritten by `R*Z1` unless `R = missing`.
"""
function _preduce2!(n::Int, m::Int, p::Int, M::AbstractMatrix{T}, N::AbstractMatrix{T},
                    Q::Union{AbstractMatrix{T},Nothing}, Z::Union{AbstractMatrix{T},Nothing}, tol::Real,
                    L::Union{AbstractVecOrMat{T},Missing} = missing, R::Union{AbstractMatrix{T},Missing} = missing;
                    fast::Bool = true, roff::Int = 0, coff::Int = 0, rtrail::Int = 0, ctrail::Int = 0,
                    withQ::Bool = true, withZ::Bool = true) where T <: BlasFloat
   #  Steps 1 and 2: QR- or SVD-decomposition based full column compression of [D C] with the UT-form preservation of E
   #  Reduce the structured pencil
   #
   #  M = [ *  * *  *  ] roff          N = [ *  * *  *  ] roff
   #      [ *  B A  *  ] n                 [ 0  0 E  *  ] n
   #      [ *  D C  *  ] p                 [ 0  0 0  *  ] p
   #      [ 0  0 0  *  ] rtrail            [ 0  0 0  *  ] rtrail
   #      coff m n ctrail                  coff m n ctrail
   #
   #  with E upper triangular and nonsingular to the following form
   #
   #  M1 = [ *   *   *    *    *  ] roff       N1 = [ *    *   *   *    *   ] roff
   #       [ *   B1  A11 A12   *  ] n-ρ             [ 0    0  E11 E12   *   ] n-ρ
   #       [ *   D1  C1  A22   *  ] ρ               [ 0    0   0  E22   *   ] ρ
   #       [ 0   0   0   C2    *  ] p               [ 0    0   0   0    *   ] p
   #       [ 0   0   0   0     *  ] rtrail          [ 0    0   0   0    *   ] rtrail
   #       coff m-τ n-ρ τ+ρ ctrail                   coff m-τ n-ρ τ+ρ ctrail
   #
   #  where τ = rank D, C2 is full column rank and E11 and E22 are upper triangular and nonsingular.

   npp = n+p
   npm = n+m
   ZERO = zero(T)
   # Step 1:
   mM = roff + npp + rtrail
   nM = coff + npm + ctrail
   ia = 1:roff+n
   ic = roff+n+1:roff+npp
   jc = coff+m+1:coff+npm
   jb = coff+1:coff+m
   jdc = 1:nM
   BE = view(M,ia,jb)
   EE = view(N,1:roff,jb)
   DC = view(M,ic,coff+1:coff+npm)
   C = view(M,ic,jc)
   D = view(M,ic,jb)

   if m > 0
      # compress D' to [D1' D2'; 0 0] = Q'*D'*P with D1 invertible
      ic = roff+n+1:roff+npp
      D = view(M,ic,jb)
      if fast
         QR = qr!(copy(D'), Val(true))
         τ = count(x -> x > tol, abs.(diag(QR.R)))
      else
         # τ = rank(D; atol = tol)
         τ = count(x -> x > tol, svdvals(D))
         QR = qr!(copy(D'), Val(true))
      end
      rmul!(BE,QR.Q)  # BE*Q
      jt = m:-1:1
      BE[:,:] = BE[:,jt]      # BE*Q*P2
      if withZ
         Z1 = view(Z,:,jb)
         rmul!(Z1,QR.Q)
         Z1[:,:] = Z1[:,jt]
      end
      ismissing(R) || ( rmul!(view(R,:,jb),QR.Q); R[:,jb] = R[:,reverse(jb)] )
      rmul!(EE,QR.Q)
      EE[:,:] = EE[:,jt]      # BE*Q*P2
      D[:,:] = [ zeros(p,m-τ)  QR.R[τ:-1:1,p:-1:1]' ]
      C[:,:] = C[QR.p,:]
      C[:,:] = reverse(C,dims=1)
      # Step 2:
      k = 1
      for i = roff+npp:-1:roff+npp+1-τ
         for jj = coff+m+1-k:coff+m+n-k
             jjp1 = jj+1
             G, r = givens(conj(M[i,jjp1]),conj(M[i,jj]),jjp1,jj)
             M[i,jjp1] = conj(r)
             M[i,jj] = ZERO
             rmul!(view(M,1:i-1,jdc),G')
             rmul!(view(N,1:i,jdc),G')
             withZ && rmul!(Z,G')
             ismissing(R) || rmul!(R,G')
         end
         k += 1
      end
   else
      τ = 0
   end
   ρ = _preduce4!(n, m-τ, p-τ, M, N, Q, Z, tol, L, R, fast = fast, coff = coff, roff = roff, rtrail = rtrail+τ, ctrail = ctrail+τ, withQ = withQ, withZ = withZ)
   if m > 0
      jrt = npm-(τ+ρ)+1:npm
      DC[:,jrt] = reverse(DC[:,jrt],dims=1)
      DC[:,jrt] = DC[invperm(QR.p),jrt]
   end
   return τ, ρ
end
"""
    _preduce3!(n::Int, m::Int, M::AbstractMatrix, N::AbstractMatrix,
               Q::Union{AbstractMatrix,Nothing}, Z::Union{AbstractMatrix,Nothing}, tol,
               L::Union{AbstractMatrix{T},Missing} = missing, R::Union{AbstractMatrix{T},Missing} = missing;
               fast = true, roff = 0, coff = 0, rtrail = 0, ctrail = 0,  withQ = true, withZ = true)

Reduce the structured pencil

        [ *  *  *   *  ] roff        [ *  *  *   *  ] roff
    M = [ 0  B  A   *  ] n       N = [ 0  0  E   *  ] n
        [ 0  *  *   *  ] rtrail      [ 0  *  *   *  ] rtrail
        coff m  n ctrail             coff m  n ctrail

with `E` upper triangular and nonsingular to the following form `M1 - λN1 = Q1'*(M - λN)*Z1` with

         [ *  *   *   *     *  ] roff        [ *  *  *   *     *  ] roff
    M1 = [ 0  B1  A11 A12   *  ] ρ      N1 = [ 0  0  E11 E12   *  ] ρ
         [ 0  0   A21 A22   *  ] n-ρ         [ 0  0  0   E22   *  ] n-ρ
         [ 0  *   *   *     *  ] rtrail      [ 0  *  *   *     *  ] rtrail
         coff m   ρ   n-ρ ctrail             coff m  ρ   n-ρ ctrail

where `B1` has full row rank `ρ` and `E11` and `E22` are upper triangular and nonsingular.
The performed orthogonal or unitary transformations are accumulated in `Q` (i.e., `Q <- Q*Q1`), if `withQ = true`, and
`Z` (i.e., `Z <- Z*Z1`), if `withZ = true`. The rank decisions use the absolute tolerance `tol` for the nonzero elements of `M`.

The  matrix `L` is overwritten by `Q1'*L` unless `L = missing` and the  matrix `R` is overwritten by `R*Z1` unless `R = missing`.
"""
function _preduce3!(n::Int, m::Int, M::AbstractMatrix{T}, N::AbstractMatrix{T},
                    Q::Union{AbstractMatrix{T},Nothing}, Z::Union{AbstractMatrix{T},Nothing}, tol::Real,
                    L::Union{AbstractVecOrMat{T},Missing} = missing, R::Union{AbstractMatrix{T},Missing} = missing;
                    fast::Bool = true, roff::Int = 0, coff::Int = 0, rtrail::Int = 0, ctrail::Int = 0,
                    withQ::Bool = true, withZ::Bool = true) where T <: BlasFloat
   #  Step 3: QR- or SVD-decomposition based full row compression of B and UT-form preservation of E
   #  Reduce the structured pencil
   #
   #       [ *  *  *   *  ] roff        [ *  *  *   *  ] roff
   #   M = [ 0  B  A   *  ] n       N = [ 0  0  E   *  ] n
   #       [ 0  *  *   *  ] rtrail      [ 0  *  *   *  ] rtrail
   #       coff m  n ctrail             coff m  n ctrail
   #
   #  with E upper triangular and nonsingular to the following form
   #
   #       [ *  *   *   *     *  ] roff        [ *  *  *   *     *  ] roff
   #  M1 = [ 0  B1  A11 A12   *  ] ρ      N1 = [ 0  0  E11 E12   *  ] ρ
   #       [ 0  0   A21 A22   *  ] n-ρ         [ 0  0  0   E22   *  ] n-ρ
   #       [ 0  *   *   *     *  ] rtrail      [ 0  *  *   *     *  ] rtrail
   #       coff m  n-ρ  ρ   ctrail             coff m  n-ρ ρ   ctrail
   #
   #  where B1 has full row rank and E11 and E22 are upper triangular and nonsingular.
   #
   npm = n+m
   mM = roff + n + rtrail
   nM = coff + npm + ctrail
   ZERO = zero(T)
   ib = roff+1:roff+n
   je = coff+m+1:coff+npm
   B = view(M,ib,coff+1:coff+m)
   E = view(N,ib,je)
   if fast
      ρ = 0
      nrm = similar(real(M),m)
      jp = Vector(1:m)
      nm = min(n,m)
      for j = 1:nm
         for l = j:m
            nrm[l] = norm(B[j:n,l])
         end
         nrmax, ind = findmax(nrm[j:m])
         ind += j-1
         if nrmax < tol
            break
         else
            ρ += 1
         end
         if ind != j
            (jp[j], jp[ind]) = (jp[ind], jp[j])
            (B[:,j],B[:,ind]) = (B[:,ind],B[:,j])
         end
         for ii = n:-1:j+1
             iim1 = ii-1
             G, B[iim1,j] = givens(B[iim1,j],B[ii,j],iim1,ii)
             B[ii,j] = ZERO
             lmul!(G,view(M,ib,coff+j+1:nM))
             lmul!(G,view(N,ib,coff+m+iim1:nM))
             withQ && rmul!(view(Q,:,ib),G')
             ismissing(L) || lmul!(G,view(L,ib,:))
             G, r = givens(conj(E[ii,ii]),conj(E[ii,iim1]),ii,iim1)
             E[ii,ii] = conj(r)
             E[ii,iim1] = ZERO
             rmul!(view(N,1:roff+iim1,je),G')
             withZ && rmul!(view(Z,:,je),G')
             rmul!(view(M,:,je),G')
             ismissing(R) || rmul!(view(R,:,je),G')
         end
      end
      B[:,:] = [ B[1:ρ,invperm(jp)]; zeros(T,n-ρ,m) ]
   else
      if n > m
        for j = 1:m
          for ii = n:-1:j+1
            iim1 = ii-1
            G, B[iim1,j] = givens(B[iim1,j],B[ii,j],iim1,ii)
            B[ii,j] = ZERO
            lmul!(G,view(M,ib,coff+j+1:nM))
            lmul!(G,view(N,ib,coff+m+iim1:nM))
            withQ && rmul!(view(Q,:,ib),G')
            ismissing(L) || lmul!(G,view(L,ib,:))
            G, r = givens(conj(E[ii,ii]),conj(E[ii,iim1]),ii,iim1)
            E[ii,ii] = conj(r)
            E[ii,iim1] = ZERO
            rmul!(view(N,1:roff+iim1,je),G')
            withZ && rmul!(view(Z,:,je),G')
            rmul!(view(M,:,je),G')
            ismissing(R) || rmul!(view(R,:,je),G')
         end
        end
      end
      mn = min(n,m)
      if mn > 0
         ics = 1:mn
         jcs = 1:m
         SVD = svd(B[ics,jcs], full = true)
         ρ = count(x -> x > tol, SVD.S)
         if ρ == mn
            return ρ
         else
            B[ics,jcs] = [ Diagonal(SVD.S[1:ρ])*SVD.Vt[1:ρ,:]; zeros(T,mn-ρ,m) ]
            if ρ == 0
               return ρ
            end
         end
         ibt = roff+1:roff+mn
         jt = coff+m+1:nM
         withQ && (Q[:,ibt] = Q[:,ibt]*SVD.U)
         N[ibt,jt] = SVD.U'*N[ibt,jt]
         M[ibt,jt] = SVD.U'*M[ibt,jt]
         ismissing(L) || (L[ibt,:] = SVD.U'*L[ibt,:])
         tau = similar(N,mn)
         jt1 = coff+m+1:coff+m+mn
         E11 = view(N,ibt,jt1)
         LinearAlgebra.LAPACK.gerqf!(E11,tau)
         T <: Complex ? tran = 'C' : tran = 'T'
         LinearAlgebra.LAPACK.ormrq!('R',tran,E11,tau,view(M,:,jt1))
         withZ && LinearAlgebra.LAPACK.ormrq!('R',tran,E11,tau,view(Z,:,jt1))
         LinearAlgebra.LAPACK.ormrq!('R',tran,E11,tau,view(N,1:roff,jt1))
         ismissing(R) || LinearAlgebra.LAPACK.ormrq!('R',tran,E11,tau,view(R,:,jt1))
         triu!(E11)
      else
         ρ = 0
      end
   end
   return ρ
end
"""
    _preduce4!(n::Int, m::Int, p::Int, M::AbstractMatrix, N::AbstractMatrix,
               Q::Union{AbstractMatrix,Nothing}, Z::Union{AbstractMatrix,Nothing}, tol,
               L::Union{AbstractMatrix{T},Missing} = missing, R::Union{AbstractMatrix{T},Missing} = missing;
               fast = true, roff = 0, coff = 0, rtrail = 0, ctrail = 0,  withQ = true, withZ = true)

Reduce the structured pencil

    M = [ *  *  *   *  ] roff          N = [ *  *  *   *  ] roff
        [ 0  B  A   *  ]  n                [ 0  0  E   *  ]  n
        [ 0  0  C   *  ]  p                [ 0  0  0   *  ]  p
        [ 0  *  *   *  ] rtrail            [ 0  *  *   *  ] rtrail
        coff m  n ctrail                   coff m  n ctrail

with `E` upper triangular and nonsingular to the following form `M1 - λN1 = Q1'*(M - λN)*Z1` with

    M1 = [ *  *   *   *    * ] roff         N1 = [ *  *  *   *    * ] roff
         [ 0  B1  A11 A12  * ]  n-ρ              [ 0  0  E11 E12  * ]  n-ρ
         [ 0  B2  A21 A22  * ]  ρ                [ 0  0  0   E22  * ]  ρ
         [ 0  0   0   C1   * ]  p                [ 0  0  0   0    * ]  p
         [ 0  *   *   *    * ] rtrail            [ 0  *  *   *    * ] rtrail
         coff m  n-ρ  ρ ctrail                   coff m n-ρ  ρ ctrail

where `C1` has full column rank and `E11` and `E22` are upper triangular and nonsingular.
The performed orthogonal or unitary transformations are accumulated in `Q` (i.e., `Q <- Q*Q1`), if `withQ = true`, and
`Z` (i.e., `Z <- Z*Z1`), if `withZ = true`. The rank decisions use the absolute tolerance `tol` for the nonzero elements of `M`.

The  matrix `L` is overwritten by `Q1'*L` unless `L = missing` and the  matrix `R` is overwritten by `R*Z1` unless `R = missing`.
"""
function _preduce4!(n::Int, m::Int, p::Int, M::AbstractMatrix{T},N::AbstractMatrix{T},
                    Q::Union{AbstractMatrix{T},Nothing}, Z::Union{AbstractMatrix{T},Nothing}, tol::Real,
                    L::Union{AbstractVecOrMat{T},Missing} = missing, R::Union{AbstractMatrix{T},Missing} = missing;
                    fast::Bool = true, roff::Int = 0, coff::Int = 0, rtrail::Int = 0, ctrail::Int = 0,
                    withQ::Bool = true, withZ::Bool = true) where T <: BlasFloat
   #  Step 4: QR- or SVD-decomposition based full column compression of C and UT-form preservation of E
   #  Reduce the structured pencil
   #
   #  M = [ *  *  *   *  ] roff          N = [ *  *  *   *  ] roff
   #      [ 0  B  A   *  ]  n                [ 0  0  E   *  ]  n
   #      [ 0  0  C   *  ]  p                [ 0  0  0   *  ]  p
   #      [ 0  *  *   *  ] rtrail            [ 0  *  *   *  ] rtrail
   #      coff m  n ctrail                   coff m  n ctrail
   #
   #  with E upper triangular and nonsingular to the following form
   #
   #  M1 = [ *  *   *   *    * ] roff         N1 = [ *  *  *   *    * ] roff
   #       [ 0  B1  A11 A12  * ]  n-ρ              [ 0  0  E11 E12  * ]  n-ρ
   #       [ 0  B2  A21 A22  * ]  ρ                [ 0  0  0   E22  * ]  ρ
   #       [ 0  0   0   C1   * ]  p                [ 0  0  0   0    * ]  p
   #       [ 0  *   *   *    * ] rtrail            [ 0  *  *   *    * ] rtrail
   #       coff m  n-ρ  ρ ctrail                   coff m n-ρ  ρ ctrail
   #
   #  where C1 has full column rank and E11 and E22 are upper triangular and nonsingular.
   npp = n+p
   npm = n+m
   mM = roff + npp + rtrail
   nM = coff + npm + ctrail
   ZERO = zero(T)
   ie = roff+1:roff+n
   ic = roff+n+1:roff+npp
   jc = coff+m+1:coff+npm
   C = view(M,ic,jc)
   E = view(N,ie,jc)
   AE = view(M,1:roff+n,jc)
   if fast
      ρ = 0
      nrm = similar(real(M),p)
      jp = Vector(1:p)
      np = min(n,p)
      for i = 1:np
         ii = p-i+1
         for l = 1:ii
            nrm[l] = norm(C[l,1:n-i+1])
         end
         nrmax, ind = findmax(nrm[1:ii])
         if nrmax < tol
            break
         else
            ρ += 1
         end
         if ind != ii
            (jp[ii], jp[ind]) = (jp[ind], jp[ii])
            (C[ii,:],C[ind,:]) = (C[ind,:],C[ii,:])
         end
         for jj = 1:n-i
             jjp1 = jj+1
             G, r = givens(conj(C[ii,jjp1]),conj(C[ii,jj]),jjp1,jj)
             C[ii,jjp1] = conj(r)
             C[ii,jj] = ZERO
             rmul!(view(C,1:ii-1,:),G')
             rmul!(AE,G')
             rmul!(view(N,1:roff+jjp1,jc),G')
             withZ && rmul!(view(Z,:,jc),G')
             ismissing(R) || rmul!(view(R,:,jc),G')
             G, E[jj,jj] = givens(E[jj,jj],E[jjp1,jj],jj,jjp1)
             E[jjp1,jj] = ZERO
             lmul!(G,view(N,ie,coff+m+jjp1:nM))
             withQ && rmul!(view(Q,:,ie),G')
             lmul!(G,view(M,ie,coff+1:nM))
             ismissing(L) || lmul!(G,view(L,ie,:))
         end
      end
      C[:,1:n] = [ zeros(T,p,n-ρ)  C[invperm(jp),n-ρ+1:n]];
   else
      if n > p
         for i = 1:p
             ii = p-i+1
             for jj = 1:n-i
                jjp1 = jj+1
                G, r = givens(conj(C[ii,jjp1]),conj(C[ii,jj]),jjp1,jj)
                C[ii,jjp1] = conj(r)
                C[ii,jj] = ZERO
                rmul!(view(C,1:ii-1,:),G')
                rmul!(AE,G')
                rmul!(view(N,1:roff+jjp1,jc),G')
                withZ && rmul!(view(Z,:,jc),G')
                ismissing(R) || rmul!(view(R,:,jc),G')
                G, E[jj,jj] = givens(E[jj,jj],E[jjp1,jj],jj,jjp1)
                E[jjp1,jj] = ZERO
                lmul!(G,view(N,ie,coff+m+jjp1:nM))
                withQ && rmul!(view(Q,:,ie),G')
                lmul!(G,view(M,ie,coff+1:nM))
                ismissing(L) || lmul!(G,view(L,ie,:))
               end
         end
      end
      pn = min(n,p)
      if pn > 0
         ics = 1:p
         jcs = n-pn+1:n
         SVD = svd(C[ics,jcs], full = true)
         ρ = count(x -> x > tol, SVD.S)
         if ρ == pn
            return ρ
         else
            Q1 = reverse(SVD.U,dims=2)
            C[ics,jcs] = [ zeros(T,p,pn-ρ) Q1[:,p-ρ+1:end]*Diagonal(reverse(SVD.S[1:ρ])) ]
            if ρ == 0
               return ρ
            end
         end
         Z1 = reverse(SVD.V,dims=2)
         jt = coff+npm-pn+1:coff+npm
         withZ && (Z[:,jt] = Z[:,jt]*Z1)
         M[1:roff+n,jt] = M[1:roff+n,jt]*Z1
         N[1:roff+n,jt] = N[1:roff+n,jt]*Z1    # more efficient computation possible
         ismissing(R) || (M[:,jt] = M[:,jt]*Z1)
         it = roff+n-pn+1:roff+n
         jt1 = coff+n+m+1:nM
         tau = similar(N,pn)
         E22 = view(N,it,jt)
         LinearAlgebra.LAPACK.geqrf!(E22,tau)
         T <: Complex ? tran = 'C' : tran = 'T'
         LinearAlgebra.LAPACK.ormqr!('L',tran,E22,tau,view(M,it,coff+1:nM))
         withQ && LinearAlgebra.LAPACK.ormqr!('R','N',E22,tau,view(Q,:,it))
         LinearAlgebra.LAPACK.ormqr!('L',tran,E22,tau,view(N,it,jt1))
         ismissing(L) || LinearAlgebra.LAPACK.ormqr!('L',tran,E22,tau,view(L,it,:))
         triu!(E22)
      else
         ρ = 0
      end
   end
   return ρ
end


module NumericFuns

    using Compat

    export

    # mathfuns
    sqr, rcp, rsqrt, rcbrt, xlogx, xlogy,
    sigmoid, logistic, logit, softplus, invsoftplus, logsumexp,

    # functors (Note: export of specific functors are in functors.jl)
    Functor, UnaryFunctor, BinaryFunctor, TernaryFunctor,
    evaluate, @functor1, @functor2,

    # rtypes
    fptype, arithtype, result_type

    # source files

    include("mathfuns.jl")
    include("functors.jl")
    include("rtypes.jl")

end # module


# DESCRIPTION
# Computes the linearized dynamics of a nonlinear system (acfun), given the
# trim state and control vectors (x0 and u0).
#
# INPUT
# - acfun: name of system dynamics function
# - x0: trim state vector
# - u0: trim control vector
#
# OUTPUT
# - A, B, C, D: coefficient matrices of linearized dynamics
#
# ------------------------------------------------------------------------------

function linearize(acfun,x0,u0)
    #xdot0=H60!(xdot_in,x0,u0,t)
    #xdot0=acfun(xdot_in,x0,u0,t)
    A=zeros(NSTATES,NSTATES)
    B=zeros(NSTATES,NCTRLS)
    C=zeros(NOUT,NSTATES)
    D=zeros(NOUT,NCTRLS)
    # A and B matrices
    x_p=zeros(NSTATES)
    u_p=zeros(NCTRLS)
    for k=1:NSTATES
        x_p[:]=x0[:]
        x_p[k]=x_p[k]+DELXLIN[k]
        xdot_p1, y_p1=acfun(x_p,u0)
        x_p[k]=x_p[k]-2*DELXLIN[k]
        xdot_p2, y_p2=acfun(x_p,u0)
        #
        A[:,k]=(xdot_p1-xdot_p2)/(2*DELXLIN[k])
        C[:,k]=(y_p1-y_p2)/(2*DELXLIN[k])
    end
    # C and D matrices
    for k=1:NCTRLS
        u_p[:]=u0[:]
        u_p[k]=u_p[k]+DELCLIN[k]
        xdot_p1, y_p1=acfun(x0,u_p)
        u_p[k]=u_p[k]-2*DELCLIN[k]
        xdot_p2, y_p2=acfun(x0,u_p)
        B[:,k]=(xdot_p1-xdot_p2)/(2*DELCLIN[k])
        D[:,k]=(y_p1-y_p2)/(2*DELCLIN[k])
    end
    #
    return A, B, C, D
end


using Test

using StaticArrays
using CompScienceMeshes

#include(Pkg.dir("CompScienceMeshes","examples","matlab_patches.jl"))

m1 = meshrectangle(1.0, 1.0, 0.5)
m2 = meshrectangle(1.0, 1.0, 0.5)
translate!(m2, point(0,1,0))
#m2.vertices += point(0.0, 1.0, 0.0)

m = weld(m1, m2)

@test numcells(skeleton(m,0)) == 15

## The mysterious case of the cuboid
using CompScienceMeshes
using Test

width, height, depth = 1.0, 1.0, 1.0

h = 1/4

m = meshrectangle(width, height, h, 3)

m_front = rotate(m, 0.5*pi*[1,0,0])
m_left  = rotate(m, -0.5*pi*[0,1,0])

m_back  = translate(m_front, point(0,height,0))
fliporientation!(m_back)
m_right = translate(m_left, point(width,0,0))
fliporientation!(m_right)

m_bottom = fliporientation(m)

m_fine = meshrectangle(width, height, h)
m_top = translate(m_fine, point(0,0,height))
m_open = weld(m_front, m_left, m_back, m_right, m_bottom)

#patch(m_top)
ot = weld(m_open, m_top)
#patch(ot)

nvo = numcells(skeleton(m_open,0))
nvt = numcells(skeleton(m_top,0))
nvm = numcells(skeleton(ot,0))
nvb = numcells(skeleton(boundary(m_top),0))
@test nvm == nvo + nvt - nvb

# test that consecutive welding happens correctly
function meshopenbox(a, h)

    base = meshrectangle(a, a, h, 3)
    front = rotate(base, point(π/2,0,0))
    back = -translate(front, point(0,a,0))
    left = rotate(base, point(0,-π/2,0))
    right = -translate(left, point(a,0,0))

    return weld(-base, front, right, back, left)
end

function meshuboidwithdivision(a, h)

    m1 = meshopenbox(a,h)
    translate!(m1, point(0,0,-a))

    m2 = deepcopy(m1)
    m2.vertices = [point(v[1],v[2],-v[3]) for v in m2.vertices]

    m3 = meshrectangle(a, a, h, 3)

    m = weld(m1,m2,m3)
    return m
end

a = 1.0
m = meshopenbox(a,a)
@test numcells(skeleton(m,0)) == 8

m = meshuboidwithdivision(a,a)
verts = skeleton(m, 0)
@test numcells(verts) == 12

##
a = h = 1.0
base = meshrectangle(a, a, h, 3)
front = rotate(base, point(π/2,0,0))
back = -translate(front, point(0,a,0))
left = rotate(base, point(0,-π/2,0))
right = -translate(left, point(a,0,0))

m1= weld(-base, front)
@test numcells(skeleton(m1,0)) == 6
m2 = weld(m1, right)
@test numcells(skeleton(m2,0)) == 7

m3 = weld(front, right)
@test numcells(skeleton(m3,0)) == 6

#patch(m)

#Test boundary weld
a = 1.0; h = 1/4
# plate = meshrectangle(a, a, h, 3,structured=false)
plate = readmesh(joinpath(@__DIR__,"assets/plate025.in"))
sheet = weld(plate,plate,boundary=true)

SV = skeleton(sheet,0)
SE = skeleton(sheet,1)
SF = skeleton(sheet,2)
PV = skeleton(plate,0)
PE = skeleton(plate,1)
PF = skeleton(plate,2)

@testset "boundary weld" begin
    @test numcells(SF) == 2*numcells(PF)
    @test numcells(SE) == 2*numcells(PE) - numcells(boundary(plate))
    @test numcells(SV) == 2*numcells(PV) - 16
    @test numcells(PV) - numcells(PE) + numcells(PF) == 1
    @test numcells(SV) - numcells(SE) + numcells(SF) == 2
end


include("preamble.jl")

# -------------------
# Suport function
# -------------------

X = VPolygon([[0.8, 2.2], [2.1, 1], [1.9, -1], [0, -2],
              [-2, -2], [-3, 0.6], [-0.8, 1.8], [0.8, 2.2]])
d = [-1.0, 1.0]

sv = @show σ(d, X)

sf = @show ρ(d, X)

H = HalfSpace(d, sf)

dn = normalize(d)
sfn = ρ(dn, X)

fig = p(xlim=(-4.1, 2.5), ylim=(-2.5, 2.5), ratio=1, xlab=L"x_1", ylab=L"x_2",
           legendfontsize=25,
           tickfont=font(25, "Times"),
           guidefontsize=25,
           xguidefont=font(25, "Times"),
           yguidefont=font(25, "Times"),
           xtick = ([-4, -2, 0, 2], [L"-4", L"-2", L"0", L"2"]),
           ytick = ([-2, 0, 2], [L"-2", L"0", L"2"]),
           bottom_margin=0mm, left_margin=0mm, right_margin=0mm, top_margin=0mm, size=(500, 390))

p!(fig, H, alpha=0.5, color=:lightblue, lw=3.0, ls=:dash, lc=:black)
p!(fig, X, color=:orange, lab="")
#p!(fig, Singleton([0.0, 0.0]), color=:black, lab="")
p!(fig, [0, dn[1]*sfn], [0, dn[2]*sfn], linecolor=:red, arrow=:both,
   linestyle=:dot, width=4, annotations=(0.6, 1.4, text(L"\rho(d\prime, X)", 28)))
p!(fig, Singleton(sv), alpha=1, markersize=10, color=:green, lab="",
   annotations=(-2.5, 0, text(L"\sigma(d, X)", 28)))
savefig(fig, "supfunc.pdf")

fig = p(xlim=(-4.1, 2.5), ylim=(-2.5, 2.5), ratio=1, xlab=L"x_1", ylab=L"x_2",
           legendfontsize=25,
           tickfont=font(25, "Times"),
           guidefontsize=25,
           xguidefont=font(25, "Times"),
           yguidefont=font(25, "Times"),
           xtick = ([-4, -2, 0, 2], [L"-4", L"-2", L"0", L"2"]),
           ytick = ([-2, 0, 2], [L"-2", L"0", L"2"]),
           bottom_margin=0mm, left_margin=0mm, right_margin=0mm, top_margin=0mm, size=(500, 390))

Xoct = overapproximate(X, OctDirections(2))
p!(fig, Xoct, color=:lightblue, alpha=0.5, lw=3.0, ls=:dash, lc=:black)
p!(fig, X, color=:orange, lab="")
#p!(fig, Singleton([0.0, 0.0]), color=:black, lab="")
#p!(fig, [sv[1], sv[1] + d[1]], [sv[2], sv[2] + d[2]], linecolor=:red, arrow=:arrow,
#   linestyle=:dot, width=8, annotations=(-3.3, 1.5, text(L"d", 25)))
#p!(fig, Singleton(sv), alpha=1, markersize=10, color=:green, lab="")
savefig(fig, "supfunc_oct.pdf")


include("globals.jl")

module update
using globals

export update_electric_field!
export update_magnetic_field!

#
# Vacuum
#
function update_magnetic_field!(ez, hy)

    # Hack
    size = length(hy)
    if length(hy) == length(ez)
        size -= 1
    end
    #
    #
    for i = 1:size
        hy[i] = hy[i] + (ez[i + 1] - ez[i]) / globals.imp0
    end
end
function update_electric_field!(ez, hy)
    for i = 2:length(ez)
        ez[i] = ez[i] + (hy[i] - hy[i-1]) * globals.imp0
    end
end

#
# Material
#

function update_magnetic_field!(ez, hy, mu)
    for i = 1:length(hy)
        hy[i] = hy[i] + (ez[i + 1] - ez[i]) / globals.imp0 / mu[i]
    end
end
function update_electric_field!(ez, hy, eps)
    for i = 2:length(ez)-1
        ez[i] = ez[i] + (hy[i] - hy[i-1]) * globals.imp0 / eps[i]
    end
end

#
# Lossy
#

function update_magnetic_field!(ez, hy, mu, chyh, chye)
    for i = 1:length(hy)
        hy[i] = chyh[i] * hy[i] + chye[i] * (ez[i + 1] - ez[i]) / globals.imp0 / mu[i]
    end
end

function update_electric_field!(ez, hy, eps, cezh, ceze)
    for i = 2:length(ez)-1
        ez[i] = ceze[i] * ez[i] + cezh[i] * (hy[i] - hy[i-1]) * globals.imp0 / eps[i]
    end
end

end


####################################################################################################
"""
$(TYPEDEF)

$(TYPEDFIELDS)

"""
@with_kw_noshow mutable struct Cusp{Tv, Tpar, Tlens, Tevr, Tevl, Tnf} <: AbstractBifurcationPoint
	"Cusp point"
	x0::Tv

	"Parameters used by the vector field."
	params::Tpar

	"Parameter axis used to compute the branch on which this cusp point was detected."
	lens::Tlens

	"Right eigenvector"
	ζ::Tevr

	"Left eigenvector"
	ζstar::Tevl

	"Normal form coefficients"
	nf::Tnf

	"Type of bifurcation"
	type::Symbol
end

type(bp::Cusp) = :Cusp

function Base.show(io::IO, bp::Cusp)
	lens1, lens2 = bp.lens
	p1 = get(bp.params, lens1)
	p2 = get(bp.params, lens2)
	println(io, "Cusp bifurcation point at ", getLensSymbol(lens1, lens2)," ≈ ($p1, $p2).")
	# avoid aliasing with user defined parameters
	p1 = :β1 == getLensSymbol(lens1) ? :p1 : :β1
	p2 = :β2 == getLensSymbol(lens2) ? :p2 : :β2
	println(io, "Normal form: $p1 + $p2⋅A + c⋅A³)")
	@unpack c = bp.nf
	println(io, "Normal form coefficients:\n c = $c")
end
####################################################################################################
"""
$(TYPEDEF)

$(TYPEDFIELDS)

"""
@with_kw_noshow mutable struct BogdanovTakens{Tv, Tpar, Tlens, Tevr, Tevl, Tnf, Tnf2} <: AbstractBifurcationPoint
	"Bogdanov-Takens point"
	x0::Tv

	"Parameters used by the vector field."
	params::Tpar

	"Parameter axis used to compute the branch on which this BT point was detected."
	lens::Tlens

	"Right eigenvectors"
	ζ::Tevr

	"Left eigenvectors"
	ζstar::Tevl

	"Normal form coefficients (basic)"
	nf::Tnf

	"Normal form coefficients (detailed)"
	nfsupp::Tnf2

	"Type of bifurcation"
	type::Symbol
end

type(bp::BogdanovTakens) = :BogdanovTakens

function Base.show(io::IO, bp::BogdanovTakens)
	lens1, lens2 = bp.lens
	p1 = get(bp.params, lens1)
	p2 = get(bp.params, lens2)
	println(io, "Bogdanov-Takens bifurcation point at ", getLensSymbol(lens1, lens2)," ≈ ($p1, $p2).")
	# avoid aliasing with user defined parameters
	p1 = :β1 == getLensSymbol(lens1) ? :p1 : :β1
	p2 = :β2 == getLensSymbol(lens2) ? :p2 : :β2
	println(io, "Normal form (B, $p1 + $p2⋅B + b⋅A⋅B + a⋅A²)")
	@unpack a,b = bp.nf
	println(io, "Normal form coefficients:\n a = $a\n b = $b")
	println(io, "\nYou can call various predictors:\n - predictor(::BogdanovTakens, ::Val{:HopfCurve}, ds)\n - predictor(::BogdanovTakens, ::Val{:FoldCurve}, ds)\n - predictor(::BogdanovTakens, ::Val{:HomoclinicCurve}, ds)")
end
####################################################################################################
"""
$(TYPEDEF)

$(TYPEDFIELDS)

"""
mutable struct Bautin{Tv, Tpar, Tlens, Tevr, Tevl, Tnf} <: AbstractBifurcationPoint
	"Bautin point"
	x0::Tv

	"Parameters used by the vector field."
	params::Tpar

	"Parameter axis used to compute the branch on which this Bautin point was detected."
	lens::Tlens

	"Right eigenvectors"
	ζ::Tevr

	"Left eigenvectors"
	ζstar::Tevl

	"Normal form coefficients"
	nf::Tnf

	"Type of Bautin bifurcation"
	type::Symbol
end

type(bp::Bautin) = :Bautin

function Base.show(io::IO, bp::Bautin)
	lens1, lens2 = bp.lens
	p1 = get(bp.params, lens1)
	p2 = get(bp.params, lens2)
	println(io, "Bautin bifurcation point at ", getLensSymbol(lens1, lens2)," ≈ ($p1, $p2).")
	println(io, "ω = ", bp.nf.ω)
	println(io, "Second lyapunov coefficient l2 = ", bp.nf.l2)
	println(io, "Normal form: i⋅ω⋅u + l2⋅u⋅|u|⁴")
	println(io, "Normal form coefficients (detailed):")
	println(io, bp.nf)
end


using ModelingToolkit
using Test

# Basic electric components
@parameters t
function Pin(name)
    @variables v(t) i(t)
    ODESystem(Equation[], t, [v, i], [], name=name, default_u0=[v=>1.0, i=>1.0])
end

function Ground(name)
    p = Pin(name)
    eqs = [p.v ~ 0]
    ODESystem(eqs, t, [], [], systems=[p], name=name)
end

function OnePort(name)
    p = Pin(:p)
    n = Pin(:n)
    @variables v(t) i(t)
    eqs = [
           v ~ p.v - n.v
           0 ~ p.i + n.i
           i ~ p.i
          ]
    v, i, ODESystem(eqs, t, [v, i], [], systems=[p, n], name=name)
end

function ConstantVoltage(name; V = 1.0)
    val = V
    v, i, op = OnePort(name)
    @parameters V
    push!(op.eqs, v ~ V)
    V = ModelingToolkit.value(V)
    push!(op.ps, V)
    op.default_p[V] = val
    op
end

function Resistor(name; R = 1.0)
    val = R
    v, i, op = OnePort(name)
    @parameters R
    push!(op.eqs, v ~ R * i)
    R = ModelingToolkit.value(R)
    push!(op.ps, R)
    op.default_p[R] = val
    op
end

function Capacitor(name; C = 1.0)
    val = C
    v, i, op = OnePort(name)
    @parameters C
    D = Differential(t)
    push!(op.eqs, D(v) ~ i / C)
    C = ModelingToolkit.value(C)
    push!(op.ps, C)
    op.default_p[C] = val
    op
end

function Inductor(name; L = 1.0)
    val = L
    v, i, op = OnePort(name)
    @parameters L
    D = Differential(t)
    push!(op.eqs, D(i) ~ v / L)
    L = ModelingToolkit.value(L)
    push!(op.ps, L)
    op.default_p[L] = val
    op
end

R1 = Resistor(:R1)
C1 = Capacitor(:C1)
I1 = Inductor(:I1)
@test isequal(R1.p.i, (@variables R1₊p₊i(t))[1])
@parameters R
@test ModelingToolkit.default_p(flatten(R1)) == Dict(R => 1.0)
@variables n₊v(t) p₊v(t) n₊i(t) p₊i(t)
@test ModelingToolkit.default_u0(flatten(R1)) == Dict(
                                                      n₊v => 1.0,
                                                      p₊v => 1.0,
                                                      n₊i => 1.0,
                                                      p₊i => 1.0,
                                                     )


# Parts of this code were taken / derived from Graphs.jl. See LICENSE for
# licensing details.

# The Bellman Ford algorithm for single-source shortest path

###################################################################
#
#   The type that capsulates the state of Bellman Ford algorithm
#
###################################################################

struct NegativeCycleError <: Exception end

# AbstractPathState is defined in core
mutable struct BellmanFordState{V,T<:Real} <: AbstractPathState
    parents::Vector{V}
    dists::Vector{T}
end

"""
    bellman_ford_shortest_paths(g, s, distmx=weights(g))
    bellman_ford_shortest_paths(g, sources, distmx=weights(g))

Uses the [Bellman-Ford algorithm](http://en.wikipedia.org/wiki/Bellman–Ford_algorithm)
to compute shortest paths of all vertices of a `g` from a source vertex `s` (or a set of source
vertices `sources`). Returns a `BellmanFordState` with relevant traversal information.
"""
function bellman_ford_shortest_paths(
        g::AGraphOrDiGraph,
        sources::AbstractVector{Int},
        distmx::AEdgeMap{T},
    ) where T<:Real
    state = BellmanFordState(zeros(Int,nv(g)), fill(typemax(T), nv(g)))
    V = vertextype(g)
    active = Set{V}()
    for v in sources
        state.dists[v] = 0
        state.parents[v] = 0
        push!(active, v)
    end
    no_changes = false
    for i in 1:nv(g)
        no_changes = true
        new_active = Set{V}()
        for u in active
            for e in out_edges(g, u)
                v = dst(e)
                edist = distmx[e]
                if state.dists[v] > state.dists[u] + edist
                    state.dists[v] = state.dists[u] + edist
                    state.parents[v] = u
                    no_changes = false
                    push!(new_active, v)
                end
            end
        end
        if no_changes
            break
        end
        active = new_active
    end
    no_changes || throw(NegativeCycleError())
    return state
end

function bellman_ford_shortest_paths(
        g::AGraphOrDiGraph,
        sources::AbstractVector{Int})

    bellman_ford_shortest_paths(g, sources, weights(g))
end

bellman_ford_shortest_paths(
    g::AGraphOrDiGraph,
    v::Int,
    distmx::AEdgeMap=weights(g)
) = bellman_ford_shortest_paths(g, [v], distmx)

function has_negative_edge_cycle(g::AGraphOrDiGraph)
    try
        bellman_ford_shortest_paths(g, vertices(g))
    catch e
        isa(e, NegativeCycleError) && return true
    end
    return false
end


struct CartpoleCost{T} <: TO.CostFunction
    Q::Vector{T}
    R::Vector{T}
end
function TO.stage_cost(cost::CartpoleCost, x, u)
    return TO.stage_cost(cost, x) + 0.5 * cost.R[1] * u[1]^2
end
function TO.stage_cost(cost::CartpoleCost, x::AbstractVector)
    y = x[1]
    θ = x[2]
    ydot = x[3]
    θdot = x[4]
    J = cost.Q[2] * (1 .- sin(θ/2))
    J += 0.5* (cost.Q[1] * y^2 + cost.Q[3] * ydot^2 + cost.Q[4] * θdot^2)
    return J
end
TO.state_dim(::CartpoleCost) = 4
TO.control_dim(::CartpoleCost) = 1
Base.copy(cost::CartpoleCost) = CartpoleCost(copy(cost.Q), copy(cost.R))

@testset "Nonlinear Cartpole" begin
## Initialize model
model = RobotZoo.Cartpole()
N = 101
tf = 3.0

## Set up cost function
Q = [0.1,0.1,0.1,0.1]
R = [0.1]
Qf = [100,1000,100,100.]
costfun = CartpoleCost(Q,R)
costfun_term = CartpoleCost(Qf,R*0)
obj = Objective(costfun, costfun_term, N)

## Define problem
x0 = zeros(4)
xf = [0,pi,0,0]

# Solve w/ iLQR
prob = Problem(model, obj, xf, tf, x0=x0)
ilqr = Altro.iLQRSolver(prob,
    cost_tolerance=1e-3, gradient_tolerance=1e-2)
b = benchmark_solve!(ilqr)
err = states(ilqr)[end] - xf
@test err'err < 1e-3
if VERSION >= v"1.5"
    @test b.allocs == 0
end

# Add constraints
cons = ConstraintList(4,1,N)
add_constraint!(cons, GoalConstraint(xf), N)
prob = Problem(model, obj, xf, tf, x0=x0, constraints=cons)
solver = ALTROSolver(prob, cost_tolerance_intermediate=1e-2, show_summary=false)
solver.opts.ρ_primal = 1e-3
solve!(solver)
err = states(ilqr)[end] - xf
@test err'err < 1e-4

end


module Day3

function read_input(filepath)
    # Parse the file into a 1000x12 matrix of Ints
    lines = readlines(filepath)
    n_rows = length(lines)
    n_cols = length(lines[1])
    A = zeros(Int64, (n_rows, n_cols))
    for row = 1:n_rows
        for col = 1:n_cols
            A[row,col] = parse(Int64, lines[row][col])
        end
    end

    return A
end

function calculate_gamma_and_epsilon_rates(diagnostics::Matrix{Int64})
    column_sums = sum(diagnostics, dims=1)
    n_rows = size(diagnostics)[1]
    gamma_rate_binary = string("0b", join(Int(i >= n_rows/2) for i in column_sums))
    epsilon_rate_binary = string("0b", join(Int(i < n_rows/2) for i in column_sums))

    gamma_rate_decimal = parse(Int, gamma_rate_binary)
    epsilon_rate_decimal = parse(Int, epsilon_rate_binary)

    return (gamma_rate_decimal, epsilon_rate_decimal)
end

function calculate_oxygen_and_co2_ratings(diagnostics::Matrix{Int64})
    # To calculate the oxygen rating:
    # Start with the 1st column, determining the most common number in the column (tie goes to 1)
    # Discard all rows whose bit in this column are not that number.
    # Do we have only 1 row left? If not, move to the next column and repeat until we have only 1 row remaining.
    # if we cycle through all columns, start again

    n_cols = size(diagnostics)[2]

    # Calculate oxygen rating
    oxygen_rating_binary = begin
        candidates = diagnostics
        n_iter = 0
        while size(candidates)[1] > 1
            current_column = (n_iter % n_cols) + 1 # so that we can cycle through the columns indefinitely 1,2,...,n_cols,1,2,...

            n_rows = size(candidates)[1] # current number of rows remaining
            n_ones_in_column = sum(candidates[:, current_column])
            most_common_value_in_column = (n_ones_in_column / n_rows) >= 0.5 ? 1 : 0

            # Filter candidates matrix further to only include rows where current column contains most common bit
            candidates = candidates[candidates[:, current_column] .== most_common_value_in_column, :]

            n_iter += 1
        end
        join(candidates[1,:]) # collapse the lone remaining row into a string
    end

    # Calculate co2 rating
    co2_rating_binary = begin
        candidates = diagnostics
        n_iter = 0
        while size(candidates)[1] > 1
            current_column = (n_iter % n_cols) + 1 # so that we can cycle through the columns indefinitely 1,2,...,n_cols,1,2,...

            n_rows = size(candidates)[1] # current number of rows remaining
            n_ones_in_column = sum(candidates[:, current_column])
            least_common_value_in_column = (n_ones_in_column / n_rows) < 0.5 ? 1 : 0

            # Filter candidates matrix further to only include rows where current column contains most common bit
            candidates = candidates[candidates[:, current_column] .== least_common_value_in_column, :]

            n_iter += 1
        end
        join(candidates[1,:]) # collapse the lone remaining row into a string
    end

    oxygen_rating_decimal = parse(Int, string("0b", oxygen_rating_binary))
    co2_rating_decimal = parse(Int, string("0b", co2_rating_binary))
    return (oxygen_rating_decimal, co2_rating_decimal)
end

function part1_answer()
    diagnostics = read_input(joinpath(@__DIR__, "input.txt"))
    (γ, ϵ) = calculate_gamma_and_epsilon_rates(diagnostics)
    return γ * ϵ
end

function part2_answer()
    diagnostics = read_input(joinpath(@__DIR__, "input.txt"))
    (o2, co2) = calculate_oxygen_and_co2_ratings(diagnostics)
    return o2 * co2
end

end


cd(@__DIR__); include("setups/path128.jl")
gr(dpi = 200)

##
f = 1.5 * 𝚽[:, 11] + 𝚽[:, 31] .* characteristic(1:32, N) +
    𝚽[:, 61] .* characteristic(1:64, N) + 0.5 * 𝚽[:, 111]

## (a)
plot(f, c = :black, grid = false, legend = false, frame = :box, lw = 1.5)
xticks!([1; 16:16:128], vcat(string(1), [string(k) for k in 16:16:128]))
plt = plot!(xlab = latexstring("x"), ylab = latexstring("f"),
    size = (600, 400), guidefontsize = 18, xlim = [1, N])
savefig(plt, "../figs/Path128_Spectrogram_f.png")

## (b)
plot(0:N-1, 𝚽' * f, c = :black, grid = false, legend = false, frame = :box, lw = 1.5)
xticks!([0; 15:16:127], vcat(string("DC"), [string(k) for k in 15:16:127]))
plt = plot!(xlab = latexstring("l"), ylab = latexstring("g"),
    size = (600, 400), guidefontsize = 18, xlim = [1, N])
savefig(plt, "../figs/Path128_Spectrogram_g.png")


#=
Code taken from IntervalSpecialFunctions.jl which will be added as a dependency
once this has been tagged. Some modifications have been made.

The IntervalSpecialFunctions.jl package is licensed under the MIT "Expat" License:
Copyright (c) 2018: David Sanders.

Permission is hereby granted, free of charge, to any person obtaining a copy of this
software and associated documentation files (the "Software"), to deal in the Software
without restriction, including without limitation the rights to use, copy, modify,
merge, publish, distribute, sublicense, and/or sell copies of the Software, and to
permit persons to whom the Software is furnished to do so, subject to the following
conditions:

The above copyright notice and this permission notice shall be included in all copies
or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE
FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
DEALINGS IN THE SOFTWARE.
=#
for f in (:erf, :erfc)

    @eval function($f)(x::BigFloat, r::RoundingMode)
        setrounding(BigFloat, r) do
            ($f)(x)
        end
    end

    @eval ($f)(a::Interval{Float64}) = convert(Interval{Float64}, ($f)(big_val(a)))
end

function erf(a::Interval{T}) where T
    isempty(a) && return a
    @round( erf(a.lo), erf(a.hi) )
end

function erfc(a::Interval{T}) where T
    isempty(a) && return a
    @round( erfc(a.hi), erfc(a.lo) )
end

erfinv(x, r::RoundingMode{:Down}) = _erfinv(x).lo
erfinv(x, r::RoundingMode{:Up}) = _erfinv(x).hi
erfcinv(x, r::RoundingMode{:Down}) = _erfcinv(x).hi
erfcinv(x, r::RoundingMode{:Up}) = _erfcinv(x).lo

erfinv(a::BigFloat) = mid(_erfinv(a))
erfcinv(a::BigFloat) = mid(_erfcinv(a))

function _erfinv(a::T) where T
    domain = Interval{T}(-1, 1)
    a ∉ domain && return DomainError("$a is not in [-1, 1]")
    f = x -> erf(x) - a
    fp = x->2/sqrt(Interval(pi)) * exp(-x^2)
    rts = roots(f, fp, Interval{T}(-Inf, Inf))
    @assert length(rts) == 1 # && rts[1].status == :unique

    rts[1].interval
end

function erfinv(a::Interval{T}) where T
    domain = Interval{T}(-1, 1)
    a = a ∩ domain

    isempty(a) && return a
    @round(erfinv(a.lo), erfinv(a.hi))
end

function _erfcinv(a::T) where T
    domain = Interval{T}(0, 2)
    a ∉ domain && return DomainError("$a is not in [0, 2]")
    f = x -> erfc(x) - a
    fp = x -> -2/sqrt(Interval(pi)) * exp(-x^2)
    rts = roots(f, fp, Interval{T}(-Inf, Inf))
    @assert length(rts) == 1 # && rts[1].status == :unique

    rts[1].interval
end

function erfcinv(a::Interval{T}) where T
    domain = Interval{T}(0, 2)
    a = a ∩ domain

    isempty(a) && return a
    @round(erfcinv(a.hi), erfcinv(a.lo))
end


#Test library imports
using Test

#Arithmetic library imports
using IntervalArithmetic
using IntervalContractors

#Preamble
setprecision(53)
setprecision(Interval, Float64)

# Using approximate checks for validaty update later?
import Base.isapprox
isapprox(x::Interval,y::Interval) = isapprox(x.lo,y.lo,atol=1E-4) && isapprox(x.hi,y.hi,atol=1E-4)

@testset "exp2_rev_test" begin
    @test exp2_rev(∅, entireinterval(Float64))[2] == ∅
    @test exp2_rev(Interval(0.0, 1.0), ∅)[2] == ∅
    @test exp2_rev(∅, Interval(0.0, 1.0))[2] == ∅
    @test exp2_rev(Interval(-2.0, -1.0), entireinterval(Float64))[2] == ∅
    @test exp2_rev(Interval(1.0, 1.0), entireinterval(Float64))[2] == Interval(0.0, 0.0)
    @test exp2_rev(entireinterval(Float64), entireinterval(Float64))[2] == Interval(-∞, ∞)
    @test exp2_rev(Interval(-Inf, 0.0), entireinterval(Float64))[2] == ∅
end

@testset "exp10_rev_test" begin
    @test exp10_rev(∅, entireinterval(Float64))[2] == ∅
    @test exp10_rev(Interval(0.0, 1.0), ∅)[2] == ∅
    @test exp10_rev(∅, Interval(0.0, 1.0))[2] == ∅
    @test exp10_rev(Interval(-2.0, -1.0), entireinterval(Float64))[2] == ∅
    @test exp10_rev(Interval(1.0, 1.0), entireinterval(Float64))[2] == Interval(0.0, 0.0)
    @test exp10_rev(entireinterval(Float64), entireinterval(Float64))[2] == Interval(-∞, ∞)
    @test exp10_rev(Interval(-Inf, 0.0), entireinterval(Float64))[2] == ∅
end

@testset "expm1_rev_test" begin
    @test expm1_rev(∅, entireinterval(Float64))[2] == ∅
    @test expm1_rev(Interval(0.0, 1.0), ∅)[2] == ∅
    @test expm1_rev(∅, Interval(0.0, 1.0))[2] == ∅
    @test expm1_rev(Interval(-2.0, -1.0), entireinterval(Float64))[2] == ∅
    @test isapprox(expm1_rev(Interval(1.0, 1.0), entireinterval(Float64))[2],Interval(0.693147, 0.693148))
    @test expm1_rev(entireinterval(Float64), entireinterval(Float64))[2] == Interval(-∞, ∞)
    @test expm1_rev(Interval(-Inf, -1.0), entireinterval(Float64))[2] == ∅
end

@testset "log2_rev_test" begin
    @test log2_rev(∅, entireinterval(Float64))[2] == ∅
    @test log2_rev(Interval(0.0, 1.0), ∅)[2] == ∅
    @test log2_rev(∅, Interval(0.0, 1.0))[2] == ∅
    @test log2_rev(Interval(-2.0, -1.0), entireinterval(Float64))[2] == Interval(0.25, 0.5)
    @test log2_rev(Interval(1.0, 1.0), entireinterval(Float64))[2] == Interval(2.0, 2.0)
    @test log2_rev(entireinterval(Float64), entireinterval(Float64))[2] == Interval(0.0, Inf)
    @test log2_rev(Interval(-Inf, 0.0), entireinterval(Float64))[2] == Interval(0.0, 1.0)
end

@testset "log10_rev_test" begin
    @test log10_rev(∅, entireinterval(Float64))[2] == ∅
    @test log10_rev(Interval(0.0, 1.0), ∅)[2] == ∅
    @test log10_rev(∅, Interval(0.0, 1.0))[2] == ∅
    @test isapprox(log10_rev(Interval(-2.0, -1.0), entireinterval(Float64))[2], Interval(0.00999999, 0.100001))
    @test log10_rev(Interval(1.0, 1.0), entireinterval(Float64))[2] == Interval(10.0, 10.0)
    @test log10_rev(entireinterval(Float64), entireinterval(Float64))[2] == Interval(0.0, Inf)
    @test log10_rev(Interval(-Inf, 0.0), entireinterval(Float64))[2] == Interval(0.0, 1.0)
end

@testset "log1p_rev_test" begin
    @test log1p_rev(∅, entireinterval(Float64))[2] == ∅
    @test log1p_rev(Interval(0.0, 1.0), ∅)[2] == ∅
    @test log1p_rev(∅, Interval(0.0, 1.0))[2] == ∅
    @test isapprox(log1p_rev(Interval(-2.0, -1.0), entireinterval(Float64))[2], Interval(-0.864665, -0.63212))
    @test isapprox(log1p_rev(Interval(1.0, 1.0), entireinterval(Float64))[2], Interval(1.71828, 1.71829))
    @test log1p_rev(entireinterval(Float64), entireinterval(Float64))[2] == Interval(-1.0, Inf)
    @test log1p_rev(Interval(-Inf, 0.0), entireinterval(Float64))[2] == Interval(-1.0, 0.0)
end


#md # # Tutorial
#md # Notebook [download](tutorial.ipynb) [nbviewer](@__NBVIEWER_ROOT_URL__/tutorial.ipynb) [source](tutorial.jl)

using MGVI
#
using Distributions
using Random
using ValueShapes
using LinearAlgebra
using Optim

# We want to fit a 3-degree polynomial using two data sets (`a` and `b`). MGVI requires a model
# expressed as a function of the model parameters and returning an instance of the Distribution.
# In this example, since we have two sets of independent measurements, we express them as
# ValueShapes.NamedTupleDist.
#
#
# We assume errors are normally distributed with unknown covariance, which has to be learned as well.

const _x1_grid = [Float64(i)/10 for i in 1:25]
const _x2_grid = [Float64(i)/10 + 0.1 for i in 1:15]
const _common_grid = sort(vcat(_x1_grid, _x2_grid))

function _mean(x_grid, p)
    p[1]*10 .+ p[2]*40 .* x_grid .+ p[3]*600 .* x_grid.^2 .+ p[4]*80 .* x_grid.^3
end

function model(p)
    dist1 = Product(Normal.(_mean(_x1_grid, p), p[5]^2*60))
    dist2 = Product(Normal.(_mean(_x2_grid, p), p[5]^2*60))
    NamedTupleDist(a=dist1,
                   b=dist2)
end
# Here we define the ground truth of the parameters, as well as an initial guess.

const true_params =  [
 -0.3
 -1.5
 0.2
 -0.5
 0.3]

const starting_point = [
  0.2
  0.5
  -0.1
  0.3
 -0.6
];
#-
function pprintln(obj)
    show(stdout, "text/plain", obj)
    println()
end;
#-
using Plots
gr(size=(400, 300), dpi=700, fmt=:png)
#-
rng = MersenneTwister(157);

# We draw data directly from the model, using the true parameter values:

data = rand(rng, model(true_params), 1)[1];
#-
function _mean(x::Vector)
    _mean(_common_grid, x)
end

init_plots =() -> let
    truth = _mean(true_params)
    plot!(_common_grid, truth, markercolor=:blue, linecolor=:blue, label="truth")
    scatter!(_common_grid, _mean(starting_point), markercolor=:orange, markerstrokewidth=0, markersize=3, label="init")
    scatter!(vcat(_x1_grid, _x2_grid), reduce(vcat, data), markercolor=:black, markerstrokewidth=0, markersize=3, label="data")
end;

# Before we start the optimization, let's have an initial look at the data.
# It is also interesting to see how our starting guess performs.

p = plot()
init_plots()

# Now we are ready to run one iteration of the MGVI.
# The output contains an updated parameter estimate (`first_iteration.result`),
# which we can compare to the true parameters.

first_iteration = mgvi_kl_optimize_step(rng,
                                        model, data,
                                        starting_point;
                                        jacobian_func=FwdRevADJacobianFunc,
                                        residual_sampler=ImplicitResidualSampler,
                                        optim_options=Optim.Options(iterations=10, show_trace=true),
                                        residual_sampler_options=(;cg_params=(;maxiter=10)))
pprintln(hcat(first_iteration.result, true_params))
p
#-
plot_iteration = (params, label) -> let
    #error_mat = mgvi_kl_errors(full_model, params)
    #display(error_mat)
    #errors = sqrt.(error_mat[diagind(error_mat)])
    #yerr = abs.(line(common_grid, params+errors) - line(common_grid, params-errors))
    #scatter!(common_grid, line(common_grid, params), markercolor=:green, label=label, yerr=yerr)
    for sample in eachcol(params.samples)
        scatter!(_common_grid, _mean(Vector(sample)), markercolor=:gray, markeralpha=0.3, markersize=2, label=nothing)
    end
    scatter!(_common_grid, _mean(params.result), markercolor=:green, label=label)
end;

# Now let's also plot the curve corresponding to the new parameters after the first iteration:

p = plot()
init_plots()
plot_iteration(first_iteration, "first")
p
#-
plot_iteration_light = (params, counter) -> let
    scatter!(_common_grid, _mean(params.result), markercolor=:green, markersize=3, markeralpha=2*atan(counter/18)/π, label=nothing)
end;

# From the plot above we see that one iteration is not enough. Let's do 5 more steps and plot the evolution of estimates.

init_plots()
plt = scatter()
next_iteration = first_iteration
for i in 1:5
    pprintln(minimum(next_iteration.optimized))
    pprintln(hcat(next_iteration.result, true_params))
    global next_iteration = mgvi_kl_optimize_step(rng,
                                                  model, data,
                                                  next_iteration.result;
                                                  jacobian_func=FwdRevADJacobianFunc,
                                                  residual_sampler=ImplicitResidualSampler,
                                                  optim_options=Optim.Options(iterations=10, show_trace=true),
                                                  residual_sampler_options=(;cg_params=(;maxiter=10)))
    plot_iteration_light(next_iteration, i)
end
pprintln(minimum(next_iteration.optimized))
pprintln(hcat(next_iteration.result, true_params))
plt

# Finally, let's plot the last estimate and compare it to the truth. Also, notice, that gray dots represent samples from
# the approximation.

p = plot()
init_plots()
plot_iteration(next_iteration, "last")
p


module Autoencoder

export trainandsave, load

using Base.Iterators: partition
using BSON
using BSON: @save
using CUDA
using Images
using NNlib
using Parameters
using Flux
using Flux.Data.MNIST
using Flux: @epochs, mse, throttle
using Statistics

@with_kw mutable struct Args
    α::Float64 = 1e-3      # learning rate
    epochs::Int = 20       # number of epochs
    N::Int = 64            # size of the encoding (i.e. hidden layer)
    batchsize::Int = 100   # batch size for training
    num::Int = 20          # number of random digits in the sample image (UNUSED)
    throttle::Int = 1      # throttle timeout (called once every X seconds)
end

global X = MNIST.images(:test) # train using the same validation dataset

function get_processed_data(args)
    # load images and convert image of type RBG to Float
    imgs = channelview.(X)
    # partition into batches of size `batchsize`
    batches = partition(imgs, args.batchsize)
    traindata = [float(hcat(vec.(imgs)...)) for imgs in batches]
    return gpu.(traindata)
end

function train(; kwargs...)
    args = Args(; kwargs...)

    traindata = get_processed_data(args)

    @info "Constructing model..."

    # You can try to make the encoder/decoder network larger
    # Also, the output of encder is a coding of the given input.
    # In this case, the input dimension is 28^2 and the output dimension of the
    # encoder is 32. This implies that the coding is a compressed representation.
    # We can make lossy compression via this `encoder`.
    intermediate = 28^2 ÷ 2
    encoder = Chain(
        Dense(28^2, intermediate, leakyrelu) |> gpu,
        Dense(intermediate, args.N, leakyrelu) |> gpu)
    decoder = Chain(
        Dense(args.N, intermediate, leakyrelu) |> gpu,
        Dense(intermediate, 28^2, leakyrelu) |> gpu)

    # define main model as a Chain of encoder and decoder models
    m = Chain(encoder, decoder)

    @info "Training model..."
    loss(x) = mse(m(x), x)

    # callback, optimizer, and training
    callback = throttle(() -> @show(mean(loss.(traindata))), args.throttle)
    opt = ADAM(args.α)

    @epochs args.epochs Flux.train!(loss, params(m), zip(traindata), opt, cb=callback)

    return m, encoder, decoder
end

img(x::Vector) = Gray.(reshape(clamp.(x, 0, 1), 28, 28))

"""
Sample a compressed image from the model.

```julia
sample(m, 20)
```
"""
function sample(m, num=1)
    # convert image of type RGB to Float
    imgs = channelview.(X)
    # number of random digits (truth)
    before = [imgs[i] for i in rand(1:length(imgs), num)]
    # after applying autoencoder to `before` input image
    after = img.(map(x->cpu(m)(float(vec(x))), before))
    # stack `before` and `after` images them all together
    Gray.(hcat(vcat.(before, after)...))
end

function trainandsave(; kwargs...)
    m, encoder, decoder = train(; kwargs...)
    m = cpu(m)
    @save "models/autoencoder.bson" m
    encoder = cpu(encoder)
    @save "models/encoder.bson" encoder
    decoder = cpu(decoder)
    @save "models/decoder.bson" decoder
end

function load(model_filename_bson, sym=:m)
    model = BSON.load(model_filename_bson)[sym]
    return model
end

end # module


@testset "Verify evaluation of Probabilistic hermite functions" begin

    # Unscaled

    x = randn(100)
    m = 5
    dV = vander(ProHermite(m; scaled = false), 0, x)

    for i=0:5
        @test norm(dV[:,i+1] - FamilyProPolyHermite[i+1].(x) .* exp.(-x.^2/4))<1e-8
    end

    # Scaled
    dV = vander(ProHermite(m; scaled = true), 0, x)

    for i=0:5
        @test norm(dV[:,i+1] - FamilyScaledProPolyHermite[i+1].(x).*exp.(-x.^2/4))<1e-8
    end
end

@testset "Verify 1st derivative of Probabilistic hermite functions" begin
    # Unscaled
    x = randn(100)
    m = 5
    dV = vander(ProHermite(m; scaled = false), 1, x)

    for i=0:5
        F = ProHermite(i; scaled = false)
        @test norm(dV[:,i+1] - map(xi->ForwardDiff.derivative(F, xi), x) )<1e-8

        @test norm(dV[:,i+1] - (FamilyProPolyHermite[i+1].(x) .* (-0.5*x) .* exp.(-x.^2/4) +
            derivative(FamilyProPolyHermite[i+1],1).(x) .* exp.(-x.^2/4)))<1e-8
    end

    # Scaled
    x = randn(100)
    m = 5
    dV = vander(ProHermite(m; scaled = true), 1, x)

    for i=0:5
        F = ProHermite(i; scaled = true)
        @test norm(dV[:,i+1] - map(xi->ForwardDiff.derivative(F, xi), x) )<1e-8

        @test norm(dV[:,i+1] - (1/Cpro(i))*(FamilyProPolyHermite[i+1].(x) .* (-0.5*x) .* exp.(-x.^2/4) +
            derivative(FamilyProPolyHermite[i+1],1).(x) .* exp.(-x.^2/4)))<1e-8
    end
end

@testset "Verify second derivative of Probabilistic hermite functions" begin
    # Unscaled
    x = randn(100)
    m = 5
    dV = vander(ProHermite(m; scaled = false), 2, x)

    for i=0:5
        Hen = FamilyProPolyHermite[i+1].(x)
        Henp = derivative(FamilyProPolyHermite[i+1],1).(x)
        Henpp = derivative(FamilyProPolyHermite[i+1],2).(x)
        E = exp.(-x.^2/4)

        F = ProHermite(i; scaled = false)
        @test norm(dV[:,i+1] - map(xi->ForwardDiff.derivative(y->ForwardDiff.derivative(z->F(z), y), xi), x))<1e-8

        @test norm(dV[:,i+1] - (Henpp .*E - x .* Henp.*E + 0.5*Hen .* (0.5*x.^2 .- 1.0) .* E))<1e-8
    end

    # Scaled
    x = randn(100)
    m = 5
    dV = vander(ProHermite(m; scaled = true), 2, x)

    for i=0:5
        Hen = FamilyProPolyHermite[i+1].(x)
        Henp = derivative(FamilyProPolyHermite[i+1],1).(x)
        Henpp = derivative(FamilyProPolyHermite[i+1],2).(x)
        E = exp.(-x.^2/4)

        F = ProHermite(i; scaled = true)
        @test norm(dV[:,i+1] - map(xi->ForwardDiff.derivative(y->ForwardDiff.derivative(z->F(z), y), xi), x))<1e-8

        @test norm(dV[:,i+1] - (1/Cpro(i))*(Henpp .*E - x .* Henp.*E + 0.5*Hen .* (0.5*x.^2 .- 1.0) .* E))<1e-8
    end
end

@testset "Verify third derivative of Probabilistic hermite functions" begin
    # Unscaled
    x = randn(100)
    m = 5
    dV = vander(ProHermite(m; scaled = false), 3, x)

    for i=0:5
        F = ProHermite(i; scaled = false)
        @test norm(dV[:,i+1] - map(xi->Diff(F, 3)(xi),x))<1e-8
    end

    # Scaled

    x = randn(100)
    m = 5
    dV = vander(ProHermite(m; scaled = true), 3, x)

    for i=0:5
        F = ProHermite(i; scaled = true)
        @test norm(dV[:,i+1] - map(xi->Diff(F, 3)(xi),x))<1e-8
    end
end

# @testset "Verify integration of Probabilistic hermite functions" begin
#     Nx = 10
#     x = randn(10)
#
#     # Unscaled
#     dV = vander(ProHermite(8; scaled = false), -1, x)
#
#     for i=0:8
#         for j=1:Nx
#             @test abs(dV[j,i+1] - quadgk(y-> ProHermite(i)(y), 0.0, x[j], rtol=1e-12)[1])<1e-8
#         end
#     end
#
#     # Scaled
#     dV = vander(ProHermite(8; scaled = true), -1, x)
#
#     for i=0:8
#         for j=1:Nx
#             @test abs(dV[j,i+1] - quadgk(y-> ProHermite(i; scaled=true)(y), 0.0, x[j], rtol=1e-12)[1])<1e-8
#         end
#     end
#
# end


"Adapted From: https://machinelearningmastery.com/differential-evolution-from-scratch-in-python/"
#Written By Johnathan Bizzano

export diffevorandomsinglebin

#Mutate the genes
function mutate(a, b, c, scalefactor)
    return a + scalefactor * (b - c)
end

#Perform gene binomial crossover & clip the mutated genes
function clipNCrossover(mutated, lower_bound, upper_bound, target, cr)
    if rand() >= cr
        return target
    end
    if mutated < lower_bound
        return lower_bound
    elseif mutated > upper_bound
        return upper_bound
    end
    return mutated
end

function choose_individual(population, excluded_individuals...)
    choice = rand(1:length(population))
    while choice in excluded_individuals
        choice = rand(1:length(population))
    end
    choice
end

init_rand(lower_bound, upper_bound, rnginterval) = rand(lower_bound:rnginterval:upper_bound)

"Differential Evolution of DeRand1Bin algorithm. ScaleFactor ∈ [0, 2]. CrossoverRate ∈ [0, 1]"
function diffevorandomsinglebin(device::Device, error_func::Problem, initial_values::AbstractArray{T},
            lower_bound::T, upper_bound::T, population_size::Int, iterations::Int;
            scalefactor = .8, crossoverrate = .7, rnginterval = 1E-8, IsVerbose=true) where T

    (scalefactor > 2 || scalefactor < 0) && error("Scale Factor ∈ [0, 2]")
    (crossoverrate > 1 || crossoverrate < 0) && error("Cross Over Rate ∈ [0, 1]")
    (population_size < 4) && error("Population Size Must Be > 3")

    dims = length(initial_values)
    mutated = alloc(device, T, dims)

    #Create Population with Initial Values as first entry
    population = alloc(device, population_size, dims)
    copy!(view(population, 1, 1:dims), initial_values)

    #Initialize Random
    broadcast!(init_rand, view(population, 2:population_size, dims), lower_bound, upper_bound, rnginterval)

    #Init Eval
    errors = [error_func(ind) for ind in population]

    #Find the Best Init Performer
    best_idx = argmin(errors)
    best_error = errors[best_idx]
    prev_error = best_error

    for i in 1:iterations
        #Iterate all current Solutions
        for j in 1:population_size
            selected = view(population, j, 1:dims)
            #Choose Three Individuals in Population (Not Current)
            idx1 = choose_individual(population, j)
            idx2 = choose_individual(population, j, idx1)
            idx3 = choose_individual(population, j, idx1, idx2)

            broadcast!(mutate, mutated, view(population, idx1, 1:dims), view(population, idx2, 1:dims), view(population, idx3, 1:dims), scalefactor)
            broadcast!(clipNCrossover, mutated, lower_bound, upper_bound, selected, crossoverrate)

            #Check to see if crossover was better then current individual
            crossover_error = error_func(mutated)
            if crossover_error < error_func(selected)
                copy!(selected, mutated)
                errors[j] = crossover_error
            end
        end

        #Check Best Performer in this iteratation
        idx = argmin(errors)
        if errors[idx] < prev_error
            prev_error = errors[idx]
            best_error = prev_error
            best_idx = idx
        end

        (IsVerbose) && println("Iter:$i. Best Error:$best_error")
    end

    return (population[best_idx], best_error)
end


using ProjectedGPE
using Base.Test

# Run tests
tic()
include("modeorthonorm.jl")
@testset "Hermite basis tests" begin include("testHermites.jl") end
toc()


include("../src/algorithm.jl")

function unit_test_findZerosWithSubdivision(a, b, f, expected)
    pass = 1
    box = findZerosWithSubdivision(a, b, f)
    re1 = real.(box[1][1])
    im1 = imag.(box[1][1])
    re2 = real.(box[1][2])
    im2 = imag.(box[1][2])

    if (re1 < real.(expected) < re2) && (im2 < imag.(expected) < im1)
        pass = 0
    end

    return pass
end

function test_singlezeros(a, b, f, err=0.01, increment=err/100, dif=0.5)
    pass = 0
    try boxes = findZerosWithSubdivision(a, b, f, err, increment, dif)
        # Ensure every box contains exactly one zero and is within error range
        for b in boxes
            if (countZeros(b[1], b[2], f, increment, dif) != 1 ) || getError(b[1], b[2]) > err
                pass = -1
            end
        end
        return pass
    # When an exception is thrown, ensure that the box returned contains zeros
    catch e
        box = e.biggerBox
        if countZeros(box[1], box[2], f, increment, dif) <= 0
            pass = 1
        end
        return pass
    end

end

function test_multiplezeros(a, b, f, err=0.01, increment=err/100, dif=0.5)
    pass = 0
    try boxes = findZerosWithSubdivision(a, b, f, err, increment, dif)
        # Ensure every box contains at least one zero and is within error range
        for b in boxes
            if (countZeros(b[1], b[2], f, increment, dif) < 1 ) || getError(b[1], b[2]) > err
                pass = -1
            end
        end
        return pass

    # When an exception is thrown, ensure that the box returned contains zeros
    catch e
        box = e.biggerBox
        if countZeros(box[1], box[2], f, increment, dif) <= 0
            pass = 1
        end
        return pass
    end

end

function randomised_countzeros(n)
    pass = 0
    for i in 1:n
        roots = rand(-9:.1:9,6)
        function generatedPolynomial(x)
            return (x-roots[1] - roots[2]*im)*(x-roots[3] - roots[4]*im)*(x-roots[5] - roots[6]*im)
        end
        randomPolynomial = generatedPolynomial
        if countZeros(-10 + 10im,  10 - 10im, randomPolynomial, 0.001, 0.5) != 3
            print(roots)
            pass = 1
        end
    end
    return pass
end

function randomised_subdivision(n)
    passes = []
    for i in 1:n
        roots = rand(-5:.001:5,6)
        function generatedPolynomial(x)
            return (x-roots[1] - roots[2]*im)*(x-roots[3] - roots[4]*im)*(x-roots[5] - roots[6]*im)
        end
        randomPolynomial = generatedPolynomial
        push!(passes, test_singlezeros(-10 + 10im, 10 - 10im, randomPolynomial))
    end

    return passes
end


#=
Direct estimation of the probability of for a diffusion with X(0) = a satisfying
X(T) ∈ (b, ∞) for the double well potential V(x) = (x²-1)².
=#

using Statistics
using HypothesisTests
using Printf

include("doublewell_setup.jl");
opts = MDOptions(n_iters=nΔt)

Random.seed!(100);
n_trials = 10^4;
n_in_set = 0;
for j in 1:n_trials
    X = copy(x₀);
    sample_trajectory!(X, sampler, options=opts);
    global n_in_set+= f(X);
end
CI = confint(BinomialTest(n_in_set, n_trials));
@printf("95%% CI = (%g,%g)\n", CI[1], CI[2])


using Test, LinearAlgebra
using WaveProp
using WaveProp.Nystrom
using WaveProp.Geometry
using WaveProp.Integration
using WaveProp.Mesh

@testset "Basic tests" begin
    pde   = Helmholtz(;dim=3,k=1)
    Geometry.clear!()
    Ω,M   = WaveProp.IO.gmsh_sphere(dim=2)
    Γ     = boundary(Ω)
    mesh  = NystromMesh(view(M,Γ),order=1)
    𝐒     = SingleLayerOperator(pde,mesh)
    𝐃     = DoubleLayerOperator(pde,mesh)
    @test Nystrom.kernel_type(𝐒) == Nystrom.SingleLayer()
    @test Nystrom.kernel_type(𝐃) == Nystrom.DoubleLayer()
end

# this is a rought test that the Greens identities are satisfied. Note that
# since we do not correct for the singular character of the matrix, the errors
# are rather large. This only works, of course, because the kernels are weakly
# singular (integrable), and therefore the silly strategy of ignoring the
# diagonal will converge (albeit slowly).
@testset "Greens identity test" begin
    # construct interior solution
    pde  = Laplace(dim=3)
    xout = SVector(3,3,3)
    u    = (x)   -> SingleLayerKernel(pde)(xout,x)
    dudn = (x,n) -> DoubleLayerKernel(pde)(xout,x,n)
    Geometry.clear!()
    Ω,M   = WaveProp.IO.gmsh_sphere(dim=2,h=0.1)
    Γ     = boundary(Ω)
    mesh  = NystromMesh(view(M,Γ),order=1)
    γ₀u   = γ₀(u,mesh)
    γ₁u   = γ₁(dudn,mesh)
    𝐒     = SingleLayerOperator(pde,mesh) |> Matrix
    𝐃     = DoubleLayerOperator(pde,mesh) |> Matrix
    ee    = WaveProp.Nystrom.error_interior_green_identity(𝐒,𝐃,γ₀u,γ₁u) / norm(γ₀u,Inf)
    @test norm(ee,Inf) < 5e-2
end

@testset "Greens identity test in 2d" begin
    # construct interior solution
    pde  = Laplace(dim=2)
    xout = SVector(3,3)
    u    = (x)   -> SingleLayerKernel(pde)(xout,x)
    dudn = (x,n) -> DoubleLayerKernel(pde)(xout,x,n)
    Geometry.clear!()
    Ω,M   = WaveProp.IO.gmsh_disk(dim=1,h=0.01)
    Γ     = boundary(Ω)
    mesh = NystromMesh(view(M,Γ);order=1)
    γ₀u   = γ₀(u,mesh)
    γ₁u   = γ₁(dudn,mesh)
    𝐒     = SingleLayerOperator(pde,mesh)
    𝐃     = DoubleLayerOperator(pde,mesh)
    ee = WaveProp.Nystrom.error_interior_green_identity(𝐒,𝐃,γ₀u,γ₁u) / norm(γ₀u,Inf)
    @test norm(ee,Inf) < 5e-2
end