"""
    poly2pm(PM; grade = k) -> P

Build a grade `k` matrix polynomial representation `P(λ)` from a polynomial matrix, polynomial vector or scalar polynomial `PM(λ)`.

`PM(λ)` is a matrix, vector or scalar of elements of the `Polynomial` type
provided by the [Polynomials](https://github.com/JuliaMath/Polynomials.jl) package.

`P(λ)` is a grade `k` polynomial matrix of the form `P(λ) = P_1 + λ P_2 + ... + λ**k P_(k+1)`, for which
the coefficient matrices `P_l`, `l = 1, ..., k+1`, are stored in the 3-dimensional matrix `P`,
where `P[:,:,l]` contains the `l`-th coefficient matrix `P_l` (multiplying `λ**(l-1)`).
If `grade = missing`, then `k` is chosen the largest degree of the elements of `PM`.
The coefficients of the degree `d` element `(i,j)` of `PM(λ)` result in `P[i,j,1:d+1]`.
"""
function poly2pm(PM::Matrix{<:Polynomial}; grade::Union{Int,Missing} = missing)
   p, m = size(PM)
   degs = degree.(PM)
   d = maximum(degs)
   ismissing(grade) ? k = d+1 : k = max(d,grade)+1
   T = eltype(eltype(PM))
   k == 0 && (return zeros(T,p,m,1))
   P = zeros(T,p,m,k)
   for j = 1:m
      for i = 1:p
         degs[i,j] < 0 || (P[i,j,1:degs[i,j]+1] = coeffs(PM[i,j]))
      end
   end
   return P
end
function poly2pm(PM::Matrix{T}; grade::Union{Int,Missing} = missing) where T <: Number
   p, m = size(PM)
   d = 0
   ismissing(grade) ? k = d+1 : k = max(d,grade)+1
   k == 0 && (return zeros(T,p,m,1))
   if k == 1
      P = reshape(PM,p,m,1)
   else
      P = zeros(T,p,m,k)
      P[:,:,1] = PM
   end
   return P
end
function poly2pm(PM::Vector{<:Polynomial}; grade::Union{Int,Missing} = missing)
   m = length(PM)
   degs = degree.(PM)
   d = maximum(degs)
   ismissing(grade) ? k = d+1 : k = max(d,grade)+1
   T = eltype(eltype(PM))
   k == 0 && (return zeros(T,m,1,1))
   P = zeros(T,m,1,k)
   for i = 1:m
      degs[i] < 0 || (P[i,1,1:degs[i]+1] = coeffs(PM[i]))
   end
   return P
end
function poly2pm(PM::Vector{T}; grade::Union{Int,Missing} = missing) where T <: Number
   m = length(PM)
   d = 0
   ismissing(grade) ? k = d+1 : k = max(d,grade)+1
   k == 0 && (return zeros(T,m,1,1))
   if k == 1
      P = reshape(PM,m,1,1)
   else
      P = zeros(T,m,1,k)
      P[:,:,1] = PM
   end
   return P
end
function poly2pm(PM::Union{Adjoint{T,Vector{T}},Transpose{T,Vector{T}}}; grade::Union{Int,Missing} = missing) where T <: Number
   m = length(PM)
   d = 0
   ismissing(grade) ? k = d+1 : k = max(d,grade)+1
   k == 0 && (return zeros(T,1,m,1))
   adj = typeof(PM) <: Adjoint
   if k == 1
      adj ? P = reshape(conj(PM.parent),1,m,1) : P = reshape(PM.parent,1,m,1)
   else
      P = zeros(T,1,m,k)
      adj ? P[:,:,1] = reshape(conj(PM.parent),1,m,1) : P[:,:,1] = reshape(PM.parent,1,m,1)
   end
   return P
end
function poly2pm(PM::Polynomial{T}; grade::Union{Int,Missing} = missing) where T
   d = degree(PM)
   ismissing(grade) ? k = d+1 : k = max(d,grade)+1
   k == 0 && (return zeros(T,1,1,1))
   P = zeros(T,1,1,k)
   d < 0 || (P[1,1,1:d+1] = coeffs(PM))
   return P
end
function poly2pm(PM::Number; grade::Union{Int,Missing} = missing)
   T = typeof(PM)
   PM == zero(T) ? d = -1 : d = 0
   ismissing(grade) ? k = d+1 : k = max(d,grade)+1
   k == 0 && (return zeros(T,1,1,1))
   P = zeros(T,1,1,k)
   d < 0 || (P[1,1,1] = PM)
   return P
end
"""
    pm2poly(P[,var = 'x']) -> PM

Build the polynomial matrix `PM(λ)` from its matrix polynomial representation `P(λ)`.

`P(λ)` is a grade `k` polynomial matrix of the form `P(λ) = P_1 + λ P_2 + ... + λ**k P_(k+1)`, for which
the coefficient matrices `P_l`, `l = 1, ..., k+1`, are stored in the 3-dimensional matrix `P`,
where `P[:,:,l]` contains the `l`-th coefficient matrix `P_l` (multiplying `λ**(l-1)`).

`PM(λ)` is a matrix of elements of the `Polynomial` type
provided by the [Polynomials](https://github.com/JuliaMath/Polynomials.jl) package.
The element `(i,j)` of `PM(λ)` is built from the coefficients contained in `P[i,j,1:k+1]`.
The symbol to be used for the indeterminate `λ` can be specified in the optional input variable `var`.
"""
function pm2poly(PM::AbstractArray{T,3},var::Union{Char, AbstractString, Symbol}='x') where T
   m, n, k = size(PM)
   P = zeros(Polynomial{T},m,n)
   for i = 1:m
      for j = 1:n
          P[i,j] = Polynomial(PM[i,j,1:k],var)
      end
   end
   return P
end
"""
    pmdeg(P) -> deg

Determine the degree `deg` of a polynomial matrix `P(λ)`.

`P(λ)` is a grade `k` polynomial matrix of the form `P(λ) = P_1 + λ P_2 + ... + λ**k P_(k+1)`, for which
the coefficient matrices `P_i`, `i = 1, ..., k+1`, are stored in the 3-dimensional matrix `P`,
where `P[:,:,i]` contains the `i`-th coefficient matrix `P_i` (multiplying `λ**(i-1)`).
The degree of `P(λ)` is `deg = j-1`, where `j` is the largest index for which `P[:,:,j]` is nonzero. The degree of
the zero polynomial matrix is defined to be `deg = -1`.

`P(λ)` can also be specified as a matrix, vector or scalar of elements of the `Polynomial` type
provided by the [Polynomials](https://github.com/JuliaMath/Polynomials.jl) package.
The degree of `P(λ)` is the largest degree of the elements of `P(λ)`.
The degree of the zero polynomial matrix is defined to be `-1`.
"""
function pmdeg(P::AbstractArray{T,3}) where T
   for j = size(P,3):-1:1
       norm(P[:,:,j],Inf) > 0 && return j-1
   end
   return -1
end
function pmdeg(P::Union{AbstractVecOrMat{<:Polynomial},Polynomial,Number})
   typeof(P) <: Number && (P == 0 ? (return -1) : (return 0) )
   return maximum(degree.(P))
end
"""
    pmeval(P,val) -> R

Evaluate `R = P(val)` for a polynomial matrix `P(λ)`, using Horner's scheme.

`P(λ)` is a grade `k` polynomial matrix of the form `P(λ) = P_1 + λ P_2 + ... + λ**k P_(k+1)`, for which
the coefficient matrices `P_i`, `i = 1, ..., k+1`, are stored in the 3-dimensional matrix `P`,
where `P[:,:,i]` contains the `i`-th coefficient matrix `P_i` (multiplying `λ**(i-1)`).

`P(λ)` can also be specified as a matrix, vector or scalar of elements of the `Polynomial` type
provided by the [Polynomials](https://github.com/JuliaMath/Polynomials.jl) package.
"""
function pmeval(P::AbstractArray{T,3},val::Number) where {T}
   # Horner's scheme
   p, m, k1 = size(P)
   nd = pmdeg(P)+1
   S = typeof(val)
   nd == 0 && return zeros(promote_type(T,S),p,m)
   R = P[:,:,nd]*one(S)
   for k = nd-1:-1:1
      R = R*val+ P[:,:,k]
   end
   return R
end
pmeval(P::Union{AbstractVecOrMat{<:Polynomial},Polynomial},val::Number) = pmeval(poly2pm(P),val)
pmeval(P::Union{Number,AbstractVecOrMat{<:Number}},val::Number = 0) = P
"""
    pmreverse(P[,j]) -> Q

Build `Q(λ) = λ^j*P(1/λ)`, the `j`-reversal of a polynomial matrix `P(λ)` for `j ≥ deg(P(λ))`.
If `j` is not specified, the default value `j = deg(P(λ))` is used.

`P(λ)` can be specified as a grade `k` polynomial matrix of the form `P(λ) = P_1 + λ P_2 + ... + λ**k P_(k+1)`,
for which the coefficient matrices `P_i`, `i = 1, ..., k+1`, are stored in the 3-dimensional matrix `P`,
where `P[:,:,i]` contains the `i`-th coefficient matrix `P_i` (multiplying `λ**(i-1)`).

`P(λ)` can also be specified as a matrix, vector or scalar of elements of the `Polynomial` type
provided by the [Polynomials](https://github.com/JuliaMath/Polynomials.jl) package.

If deg(P(λ)), then `Q(λ)` is a grade `j` polynomial matrix of the form
`Q(λ) = Q_1 + λ Q_2 + ... + λ**j Q_(j+1)`, for which
the coefficient matrices `Q_i`, `i = 1, ..., j+1`, are stored in the 3-dimensional matrix `Q`,
where `Q[:,:,i]` contains the `i`-th coefficient matrix `Q_i` (multiplying `λ**(i-1)`).
The coefficient matrix `Q_i` is either `0` if `i ≤ j-d` or `Q_(j-d+i) = P_(d-i+1)` for `i = 1, ..., d`.
"""
function pmreverse(P::AbstractArray{T,3}, j::Int = pmdeg(P)) where T
   d = pmdeg(P)
   j < d && error("j must be at least $d")
   m, n, k1 = size(P)
   Q = zeros(eltype(P),m,n,j+1)
   Q[:,:,j-d+1:j+1] = reverse(P[:,:,1:d+1],dims=3)
   return Q
end
pmreverse(P::Union{AbstractVecOrMat{<:Polynomial},Polynomial,Number}; kwargs...) =
      pmreverse(poly2pm(P); kwargs...)
"""
    pmdivrem(N,D) -> (Q, R)

Compute the quotients in `Q(λ)` and remainders in `R(λ)` of the elementwise polynomial divisions `N(λ)./D(λ)`.

`N(λ)` is a polynomial matrix of the form `N(λ) = N_1 + λ N_2 + ... + λ**k N_(k+1)`, for which
the coefficient matrices `N_i`, `i = 1, ..., k+1` are stored in the 3-dimensional matrix `N`,
where `N[:,:,i]` contains the `i`-th coefficient matrix `N_i` (multiplying `λ**(i-1)`).

`D(λ)` is a polynomial matrix of the form `D(λ) = D_1 + λ D_2 + ... + λ**l D_(l+1)`, for which
the coefficient matrices `D_i`, `i = 1, ..., l+1`, are stored in the 3-dimensional matrix `D`,
where `D[:,:,i]` contain the `i`-th coefficient matrix `D_i` (multiplying `λ**(i-1)`).

Alternatively, `N(λ)` and `D(λ)` can be specified as matrices of elements of the `Polynomial` type
provided by the [Polynomials](https://github.com/JuliaMath/Polynomials.jl) package.

The polynomial matrices of quotients `Q(λ)` and remainders `R(λ)` are stored in the 3-dimensional
matrices `Q` and  `R`, respectively, where `Q[:,:,i]` and `R[:,:,i]` contain the `i`-th
coefficient matrix multiplying `λ**(i-1)`.
"""
function pmdivrem(N::AbstractArray{T1,3},D::AbstractArray{T2,3}) where {T1,T2}
    p, m, k = size(N)
    p1, m1, k1 = size(D)
    (p,m) == (p1, m1) || error("Numerator and denominator polynomial matrices must have the same size")

    degQ1 = 0
    degD1 = 0
    for j = 1:m
        for i = 1:p
            n1 = poldeg1(N[i,j,1:k])
            d1 = poldeg1(D[i,j,1:k1])
            d1 == 0 && error("DivideError: zero denominator polynomial")
            n1 < d1 || (degQ1 = max(degQ1,n1-d1+1))
            degD1 = max(degD1,d1)
        end
    end
    T = eltype(one(T1)/one(T2))
    R = zeros(T,p,m,max(degD1-1,1))
    Q = zeros(T,p,m,max(degQ1,1))
    for j = 1:m
        for i = 1:p
            q, r = poldivrem(N[i,j,1:k],D[i,j,1:k1])
            nq1 = poldeg1(q)
            nr1 = poldeg1(r)
            nq1 == 0 || (Q[i,j,1:nq1] = copy_oftype(q[1:nq1],T))
            nr1 == 0 || (R[i,j,1:nr1] = copy_oftype(r[1:nr1],T))
        end
    end
    return Q[:,:,1:pmdeg(Q)+1], R[:,:,1:pmdeg(R)+1]
end
pmdivrem(N::Union{AbstractVecOrMat{<:Polynomial},Polynomial,Number,AbstractVecOrMat{<:Number}},
         D::Union{AbstractVecOrMat{<:Polynomial},Polynomial,Number,AbstractVecOrMat{<:Number}}) =
         pmdivrem(poly2pm(N),poly2pm(D))
"""
     pm2lpCF1(P; grade = l) -> (M, N)

Build a strong linearization `M - λN` of a polynomial matrix `P(λ)` in the first companion Frobenius form.

`P(λ)` is a grade `k` polynomial matrix assumed of the form `P(λ) = P_1 + λ P_2 + ... + λ**k P_(k+1)`, with
the coefficient matrices `P_i`, `i = 1, ..., k+1` stored in the 3-dimensional matrix `P`,
where `P[:,:,i]` contains the `i`-th coefficient matrix `P_i` (multiplying `λ**(i-1)`).
The effective grade `l` to be used for linearization can be specified via the keyword argument `grade` as
`grade = l`, where `l` must be chosen equal to or greater than the degree of `P(λ)`.
The default value used for `l` is `l = deg(P(λ))`.

`P(λ)` can also be specified as a matrix, vector or scalar of elements of the `Polynomial` type
provided by the [Polynomials](https://github.com/JuliaMath/Polynomials.jl) package.

If `P(λ)` is a `m x n` polynomial matrix of effective grade `l` and degree `d`,
then the resulting matrix pencil `M - λN` satisfies the following conditions [1]:

(1) `M - λN` has dimension `(m+n*(l-1)) x n*l` and `M - λN` is regular if `P(λ)` is regular;

(2) `M - λN` and `P(λ)` have the same finite eigenvalues;

(3) the partial multiplicities of infinite eigenvalues of `M - λN` are in excess with `l-d` to the
partial multiplicities of the infinite eigenvalues of `P(λ)`;

(4) `M - λN` and `P(λ)` have the same number of right Kronecker indices and the right
Kronecker indices of `M - λN` are in excess with `l-1` to the right Kronecker indices of `P(λ)`;

(5) `M - λN` and `P(λ)` have the same left Kronecker structure (i.e., the same left Kronecker indices).

[1] F. De Terán, F. M. Dopico, D. S. Mackey, Spectral equivalence of polynomial matrices and
the Index Sum Theorem, Linear Algebra and Its Applications, vol. 459, pp. 264-333, 2014.
"""
function pm2lpCF1(P::AbstractArray{T,3}; grade::Int = pmdeg(P)) where T
   m, n, k1 = size(P)
   deg = pmdeg(P)
   grade < deg && error("The selected grade must be at least $deg")
   grade == -1 && (return zeros(T,m,n), nothing )
   grade == 0 && (return P[:,:,1], nothing )
   grade == 1 && (grade > deg ? (return P[:,:,1], zeros(T,m,n)) : (return P[:,:,1], -P[:,:,2]) )
   nd = n*grade
   nd1 = nd-n
   grade > deg ? (N = [ zeros(T,nd1+m,n)  [zeros(T,m,nd1); I] ]) :
                 (N = [ [P[:,:,grade+1]; zeros(T,nd1,n)]  [zeros(T,m,nd1); I] ])
   M = [ zeros(T,m,nd);  [I zeros(T,nd1,n)] ]
   deg == -1 && (return M, N)
   k = nd
   it = 1:m
   grade > deg ? ne = deg+1 : ne = max(1,deg)
   for i = 1:ne
      M[it,k-n+1:k] = -P[:,:,i]
      k -= n
   end
   return M, N
end
pm2lpCF1(P::Union{AbstractVecOrMat{<:Polynomial},Polynomial,AbstractVecOrMat{<:Number}, Number}; kwargs...) =
       pm2lpCF1(poly2pm(P); kwargs...)
"""
    pm2lpCF2(P; grade = l) -> (M, N)

Build a strong linearization `M - λN` of a polynomial matrix `P(λ)` in the second companion Frobenius form.

`P(λ)` is a grade `k` polynomial matrix assumed of the form `P(λ) = P_1 + λ P_2 + ... + λ**k P_(k+1)`, with
the coefficient matrices `P_i`, `i = 1, ..., k+1` stored in the 3-dimensional matrix `P`,
where `P[:,:,i]` contains the `i`-th coefficient matrix `P_i` (multiplying `λ**(i-1)`).
The effective grade `l` to be used for linearization can be specified via the keyword argument `grade` as
`grade = l`, where `l` must be chosen equal to or greater than the degree of `P(λ)`.
The default value used for `l` is `l = deg(P(λ))`.

`P(λ)` can also be specified as a matrix, vector or scalar of elements of the `Polynomial` type
provided by the [Polynomials](https://github.com/JuliaMath/Polynomials.jl) package.

If `P(λ)` is a `m x n` polynomial matrix of effective grade `l` and degree `d`, then the resulting matrix pencil
`M - λN` satisfies the following conditions [1]:

(1) `M - λN` has dimension `l*m x (n+(l-1)*m)` and `M - λN` is regular if `P(λ)` is regular;

(2) `M - λN` and `P(λ)` have the same finite eigenvalues;

(3) the partial multiplicities of infinite eigenvalues of `M - λN` are in excess with `l-d` to the
partial multiplicities of the infinite eigenvalues of `P(λ)`;

(4) `M - λN` and `P(λ)` have the same right Kronecker structure (i.e., the same right Kronecker indices);

(5) `M - λN` and `P(λ)` have the same number of left Kronecker indices and the left
Kronecker indices of `M - λN` are in excess with `l-1` to the left Kronecker indices of `P(λ)`.

[1] F. De Terán, F. M. Dopico, D. S. Mackey, Spectral equivalence of polynomial matrices and
the Index Sum Theorem, Linear Algebra and Its Applications, vol. 459, pp. 264-333, 2014.
"""
function pm2lpCF2(P::AbstractArray{T,3}; grade::Int = pmdeg(P)) where T
   m, n, k1 = size(P)
   deg = pmdeg(P)
   grade < deg && error("The selected grade must be at least $deg")
   grade == -1 && (return zeros(T,m,n), nothing )
   grade == 0 && (return P[:,:,1], nothing  )
   grade == 1 && (grade > deg ? (return P[:,:,1], zeros(T,m,n)) : (return P[:,:,1], -P[:,:,2]) )
   md = m*grade
   md1 = md-m
   grade > deg ? (N = [ zeros(T,md,n)  [zeros(T,m,md1); I] ]) :
                 (N = [ [P[:,:,grade+1]; zeros(T,md1,n)]  [zeros(T,m,md1); I] ])
   M = [ zeros(T,md,n)  [I; zeros(T,m,md1)] ]
   deg == -1 && (return M, N)
   k = md
   it = 1:n
   grade > deg ? ne = deg+1 : ne = max(1,deg)
   for i = 1:ne
       M[k-m+1:k,it] = -P[:,:,i]
       k -= m
   end
   return M, N
end
pm2lpCF2(P::Union{AbstractVecOrMat{<:Polynomial},Polynomial,AbstractVecOrMat{<:Number}, Number}; kwargs...) =
       pm2lpCF2(poly2pm(P); kwargs...)
"""
     pm2ls(P; contr = false, obs = false, noseig = false, minimal = false,
              fast = true, atol = 0, rtol) -> (A, E, B, C, D)

Build a structured linearization as a system matrix `S(λ)` of the form

            | A-λE | B |
     S(λ) = |------|---|
            |  C   | D |

of the polynomial matrix `P(λ)` which preserves a part of the Kronecker structure of `P(λ)`.

`P(λ)` can be specified as a grade `k` polynomial matrix of the form `P(λ) = P_1 + λ P_2 + ... + λ**k P_(k+1)`,
for which the coefficient matrices `P_i`, `i = 1, ..., k+1`, are stored in the 3-dimensional matrix `P`,
where `P[:,:,i]` contains the `i`-th coefficient matrix `P_i` (multiplying `λ**(i-1)`).

`P(λ)` can also be specified as a matrix, vector or scalar of elements of the `Polynomial` type
provided by the [Polynomials](https://github.com/JuliaMath/Polynomials.jl) package.

If `d` is the degree of `P(λ)` and `n` is the order of `A-λE`, then the computed linearization satisfies:

(1) `A-λE` is regular and `P(λ) = C*inv(λE-A)*B+D`;

(2) `rank[B A-λE] = n` (controllability) if `minimal = true` or `contr = true`, in which case
the right Kronecker structure is preserved;

(3) `rank[A-λE; C] = n` (observability)  if `minimal = true` or `contr = true`, in which case
the left Kronecker structure is preserved;

(4) `A-λE` has no non-dynamic modes if `minimal = true` or `noseig = true`.

If conditions (1)-(4) are satisfied, the linearization is called `minimal` and the resulting order `n`
is the least achievable order. If conditions (1)-(3) are satisfied, the linearization is called `irreducible`
and the resulting order `n` is the least achievable order using orthogonal similarity transformations.
For an irreducible linearization `S(λ)` preserves the pole-zero structure (finite and infinite) and the
left and right Kronecker structures of `P(λ)`.

The underlying pencil manipulation algorithms [1] and [2] to compute reduced order linearizations
employ rank determinations based on either the use of
rank revealing QR-decomposition with column pivoting, if `fast = true`, or the SVD-decomposition, if `fast = false`.
The rank decision based on the SVD-decomposition is generally more reliable, but the involved computational effort is higher.

The keyword arguments `atol` and `rtol`, specify the absolute and relative tolerances for the
nonzero coefficients of `P(λ)`, respectively.

[1] P. Van Dooreen, The generalized eigenstructure problem in linear system theory,
IEEE Transactions on Automatic Control, vol. AC-26, pp. 111-129, 1981.

[2] A. Varga, Solving Fault Diagnosis Problems - Linear Synthesis Techniques, Springer Verlag, 2017.
"""
function pm2ls(P::AbstractArray{T,3}; minimal::Bool = false, contr::Bool = false, obs::Bool = false, noseig::Bool = false,
               fast::Bool = true, atol::Real = zero(real(T)),
               rtol::Real = (min(size(P)...)*eps(real(float(one(T)))))*iszero(atol)) where T
   minimal && (contr = true; obs = true; noseig = true)
   p, m, k1 = size(P)
   nd = pmdeg(P)+1
   nd == 0 && (return zeros(T,0,0), zeros(T,0,0), zeros(T,0,m), zeros(T,p,0), zeros(T,p,m))
   D = P[:,:,1]
   nd == 1 && (return zeros(T,0,0), zeros(T,0,0), zeros(T,0,m), zeros(T,p,0), D)
   if xor(contr,obs)
      if obs
         # build an observable linearization
         n = p*nd
         E = [zeros(T,n,p) [I; zeros(T,p,p*(nd-1))]]
         B = zeros(T,n,m)
         k = p
         for i = 2:nd
             B[k+1:k+p,:] = P[:,:,i]
             k += p
         end
         C = [ -I  zeros(T,p,p*(nd-1))  ]
      else
         # build a controllable linearization
         n = m*nd
         E = [zeros(T,n,m) [I; zeros(T,m,m*(nd-1))]]
         B = [zeros(T,m*(nd-1),m); -I ]
         C = zeros(T,p,n)
         k = 0
         for i = 1:nd-1
             C[:,k+1:k+m] = P[:,:,nd-i+1]
             k += m
         end
      end
      A = Matrix{T}(I,n,n)
   else
      if p <= m
         # build an observable linearization
         n = p*nd
         E = [zeros(T,n,p) [I; zeros(T,p,p*(nd-1))]]
         B = zeros(T,n,m)
         k = p
         for i = 2:nd
             B[k+1:k+p,:] = P[:,:,i]
             k += p
         end
         C = [ -I  zeros(T,p,p*(nd-1))  ]
         if contr
            # remove uncontrollable part
            T <: BlasFloat ? T1 = T : T1 = promote_type(Float64,T)
            Er = copy_oftype(E,T1)
            Br = copy_oftype(B,T1)
            Cr = copy_oftype(C,T1)
            _, _, nr, nuc = sklf_right!(Er, Br, Cr; fast = fast, atol1 = atol, atol2 = atol, rtol = rtol, withQ = false)
            if nuc > 0
               ir = 1:nr
               # save intermediary results
               E = Er[ir,ir]
               B = Br[ir,:]
               C = Cr[:,ir]
            end
            A = Matrix{T1}(I,nr,nr)
         else
            A = Matrix{T}(I,n,n)
         end
      else
         # build a controllable linearization
         n = m*nd
         E = [zeros(T,n,m) [I; zeros(T,m,m*(nd-1))]]
         B = [zeros(T,m*(nd-1),m); -I ]
         C = zeros(T,p,n)
         k = 0
         for i = 1:nd-1
             C[:,k+1:k+m] = P[:,:,nd-i+1]
             k += m
         end
         if obs
            # remove unobservable part
            T <: BlasFloat ? T1 = T : T1 = promote_type(Float64,T)
            Er = copy_oftype(E,T1)
            Br = copy_oftype(B,T1)
            Cr = copy_oftype(C,T1)
            _, _, nr, nuo = sklf_left!(Er, Cr, Br; fast = fast, atol1 = atol, atol2 = atol, rtol = rtol, withQ = false)
            if nuo > 0
               ir = n-nr+1:n
               # save intermediary results
               E = Er[ir,ir]
               B = Br[ir,:]
               C = Cr[:,ir]
            end
            A = Matrix{T1}(I,nr,nr)
         else
            A = Matrix{T}(I,n,n)
         end
      end
   end
   if noseig
      A, E, B, C, D  = lsminreal(A,E,B,C,D,contr = false, obs = false, fast = fast, atol1 = atol, atol2 = atol, rtol = rtol)
   end
   return A, E, B, C, D
end
pm2ls(P::Union{AbstractVecOrMat{Polynomial{T}},Polynomial{T},AbstractVecOrMat{Polynomial{T,X}},Polynomial{T,X}}; kwargs...) where {T,X} =
         pm2ls(poly2pm(P); kwargs...)
pm2ls(P::Union{AbstractVecOrMat{T},Number}; kwargs...) where {T <: Number} =
         pm2ls(poly2pm(P); kwargs...)
# pm2ls(P::Union{AbstractVecOrMat{Polynomial{T,X}},Polynomial{T,X}}; kwargs...) where {T,X} =
#          pm2ls(poly2pm(P); kwargs...)
"""
     pm2lps(P; contr = false, obs = false) -> (A, E, B, F, C, G, D, H)

Build a structured linearization as a system matrix `S(λ)` of the form

            | A-λE | B-λF |
     S(λ) = |------|------|
            | C-λG | D-λH |

of the polynomial matrix `P(λ)` which preserves a part of the Kronecker structure of `P(λ)`.

`P(λ)` can be specified as a grade `k` polynomial matrix of the form `P(λ) = P_1 + λ P_2 + ... + λ**k P_(k+1)`,
for which the coefficient matrices `P_i`, `i = 1, ..., k+1`, are stored in the 3-dimensional matrix `P`,
where `P[:,:,i]` contains the `i`-th coefficient matrix `P_i` (multiplying `λ**(i-1)`).

`P(λ)` can also be specified as a matrix, vector or scalar of elements of the `Polynomial` type
provided by the [Polynomials](https://github.com/JuliaMath/Polynomials.jl) package.

If `d` is the degree of the `p x m` polynomial matrix `P(λ)`, then the computed linearization satisfies:

(1) `A-λE` is a `n x n` regular pencil, where `n = p(d-1)` if `contr = false` and `p <= m`
and `n = m(d-1)` otherwise;

(2) `P(λ) = (C-λG)*inv(λE-A)*(B-λF)+D-λH`;

(3) `rank[B-λF A-λE] = n` for any finite and infinite `λ` (strong controllability) if `contr = true`, in which case
the right Kronecker structure is preserved;

(4) `rank[A-λE; C-λG] = n` for any finite and infinite `λ` (strong observability)  if `obs = true`, in which case
the left Kronecker structure is preserved.

If conditions (1)-(4) are satisfied, the linearization is called `strongly minimal`, the resulting order `n`
is the least achievable order and `S(λ)` preserves the pole-zero structure (finite and infinite) and the
left and right Kronecker structures of `P(λ)`.

The pencil based linearization is built using the methods described in [1].

[1] A. Varga, On computing the Kronecker structure of polynomial and rational matrices using Julia, 2020,
[arXiv:2006.06825](https://arxiv.org/pdf/2006.06825).
"""
function pm2lps(P::AbstractArray{T,3}; contr::Bool = false, obs::Bool = false) where T
   p, m, k1 = size(P)
   d = pmdeg(P)
   nd = d+1
   H = zeros(T,p,m)
   d == -1 && (return zeros(T,0,0), zeros(T,0,0), zeros(T,0,m), zeros(T,0,m), zeros(T,p,0), zeros(T,p,0), H, H)
   D = P[:,:,1]
   d == 0 && (return zeros(T,0,0), zeros(T,0,0), zeros(T,0,m), zeros(T,0,m), zeros(T,p,0), zeros(T,p,0), D, H)
   d == 1 && (return zeros(T,0,0), zeros(T,0,0), zeros(T,0,m), zeros(T,0,m), zeros(T,p,0), zeros(T,p,0), D, -P[:,:,2])
   if obs || (!contr && p <= m)
      # build a strongly observable linearization
      n = p*(d-1)
      E = [zeros(T,n,p) [I; zeros(T,p,n-p)]]
      B = zeros(T,n,m)
      F = zeros(T,n,m)
      k = 0
      for i = 2:d
          B[k+1:k+p,:] = P[:,:,i]
          k += p
      end
      F[n-p+1:n,:] = -P[:,:,nd]
      C = zeros(T,p,n)
      G = [I zeros(T,p,n-p) ]
      A = Matrix{T}(I,n,n)
   else
     # build a strongly controllable linearization
     n = m*(d-1)
     E = [zeros(T,n,m) [I; zeros(T,m,n-m)]]
     B = zeros(T,n,m)
     F = [zeros(T,n-m,m); I]
     C = zeros(T,p,n)
     G = [ -P[:,:,nd] zeros(T,p,n-m)]
     k = n
     for i = 2:d
         #C[:,k-m+1:k] = P[:,:,nd-i]
         C[:,k-m+1:k] = P[:,:,i]
         k -= m
     end
     A = Matrix{T}(I,n,n)
   end
   return A, E, B, F, C, G, D, H
end
pm2lps(P::Union{AbstractVecOrMat{<:Polynomial},Polynomial,Number}; kwargs...) where {T} =
         pm2lps(poly2pm(P); kwargs...)
"""
    ls2pm(A, E, B, C, D; fast = true, atol1 = 0, atol2 = 0, gaintol = 0, rtol = min(atol1,atol2) > 0 ? 0 : n*ϵ, val) -> P

Build the polynomial matrix `P(λ) = C*inv(λE-A)*B+D` corresponding to its structured linearization

     | A-λE | B |
     |------|---|
     |  C   | D |

by explicitly determining for each polynomial entry, its coefficients from its roots and a corresponding gain.

The keyword arguments `atol1` and `atol2` specify the absolute tolerances for the elements of `A`, `B`, `C`, `D`, and,
respectively, of `E`, and `rtol` specifies the relative tolerances for the nonzero elements of `A`, `B`, `C`, `D` and `E`.
The default relative tolerance is `(n+1)*ϵ`, where `n` is the size of the size dimension of `A`, and `ϵ` is the
machine epsilon of the element type of coefficients of `A`.

The keyword argument `gaintol` specifies the threshold for the magnitude of the nonzero elements of the gain matrix
`C*inv(γE-A)*B+D`, where `γ = val` if `val` is a number or `γ` is a randomly chosen complex value of unit magnitude,
if `val = missing`. Generally, `val` should not be a root of any of entries of `P`.
"""
function ls2pm(A::AbstractMatrix, E::AbstractMatrix, B::AbstractMatrix, C::AbstractMatrix, D::AbstractMatrix;
                fast::Bool = true, atol1::Real = zero(real(eltype(A))), atol2::Real = zero(real(eltype(E))),
                gaintol::Real = zero(real(eltype(A))), val::Union{Number,Missing} = missing,
                rtol::Real = ((size(A,1)+1)*eps(real(float(one(eltype(A))))))*iszero(min(atol1,atol2)))
   n = LinearAlgebra.checksquare(A)
   (n,n) == size(E) || throw(DimensionMismatch("A and E must have the same dimensions"))
   p, m = size(D)
   (n,m) == size(B) || throw(DimensionMismatch("A, B and D must have compatible dimensions"))
   (p,n) == size(C) || throw(DimensionMismatch("A, C and D must have compatible dimensions"))

   T = promote_type(eltype(A),eltype(E), eltype(B),eltype(C),eltype(D))
   T <: BlasFloat || (T = promote_type(Float64,T))
   compl = T <: Complex

   P = zeros(T,p,m,n+1)
   ismissing(val) && (val = exp(rand()*im))
   A1, E1, B1, C1, D1 = lsminreal2(A, E, B, C, D, infinite = false, noseig = false, atol1 = atol1, atol2 = atol2)
   Pval = lseval(A1, E1, B1, C1, D1, val)
   isunimodular(A1, E1, atol1 = atol1, atol2 = atol2, rtol = rtol) ||
                error("The given linearization cannot be converted to a polynomial form")
   for i = 1:p
       for j = 1:m
           if abs(Pval[i,j]) > gaintol
              zer, iz, = spzeros(A1, E1, B1[:,j:j], C1[i:i,:], D1[i:i,j:j];
                                 fast = fast, atol1 = atol1, atol2 = atol2, rtol = rtol)
              c, pval = polcoeffval(zer[1:(length(zer)-sum(iz))],val)
              P[i,j,1:length(c)] = compl ? c*(Pval[i,j]/pval) : real(c*(Pval[i,j]/pval))
           end
       end
   end
   return P[:,:,1:pmdeg(P)+1]
end
"""
    lps2pm(A, E, B, F, C, G, D, H; fast = true, atol1 = 0, atol2 = 0, gaintol = 0, rtol = min(atol1,atol2) > 0 ? 0 : n*ϵ, val) -> P

Build the polynomial matrix `P(λ) = (C-λG)*inv(λE-A)*(B-λF)+D-λH` corresponding to its structured linearization

     | A-λE | B-λF |
     |------|------|
     | C-λG | D-λH |

by explicitly determining for each polynomial entry, its coefficients from its roots and corresponding gain.

The keyword arguments `atol1` and `atol2` specify the absolute tolerances for the elements of `A`, `B`, `C`, `D`, and of
`E`, `F`, `G`, `H`, respectively,  and `rtol` specifies the relative tolerances for the nonzero elements of
`A`, `B`, `C`, `D`, `E`, F`, `G`, `H`.
The default relative tolerance is `(n+2)*ϵ`, where `n` is the size of the size dimension of `A`, and `ϵ` is the
machine epsilon of the element type of coefficients of `A`.

The keyword argument `gaintol` specifies the threshold for the magnitude of the nonzero elements of the gain matrix
`C*inv(γE-A)*B+D`, where `γ = val` if `val` is a number or `γ` is a randomly chosen complex value of unit magnitude,
if `val = missing`. Generally, `val` should not be a root of any of entries of `P`.
"""
function lps2pm(A::AbstractMatrix, E::AbstractMatrix,B::AbstractMatrix, F::AbstractMatrix,
   C::AbstractMatrix, G::AbstractMatrix, D::AbstractMatrix, H::AbstractMatrix;
   fast::Bool = true, atol1::Real = zero(real(eltype(A))), atol2::Real = zero(real(eltype(E))),
   gaintol::Real = zero(real(eltype(A))), val::Union{Number,Missing} = missing,
   rtol::Real = ((size(A,1)+2)*eps(real(float(one(eltype(A))))))*iszero(min(atol1,atol2)))
   n = LinearAlgebra.checksquare(A)
   (n,n) == size(E) || throw(DimensionMismatch("A and E must have the same dimensions"))
   p, m = size(D)
   (n,m) == size(B) || throw(DimensionMismatch("A, B and D must have compatible dimensions"))
   (p,n) == size(C) || throw(DimensionMismatch("A, C and D must have compatible dimensions"))
   (n,m) == size(F) || throw(DimensionMismatch("B and F must have the same dimensions"))
   (p,n) == size(G) || throw(DimensionMismatch("C and G must have the same dimensions"))
   (p,m) == size(H) || throw(DimensionMismatch("D and H must have the same dimensions"))
   T = promote_type(eltype(A), eltype(B), eltype(C), eltype(D), eltype(E), eltype(F), eltype(G), eltype(H))
   T <: BlasFloat || (T = promote_type(Float64,T))

   compl = T <: Complex
   P = zeros(T,p,m,n+2)
   ismissing(val) && (val = exp(rand()*im))
   A1, E1, B1, F1, C1, G1, D1, H1, V1, W1 = lpsminreal(A, E, B, F, C, G, D, H, atol1 = atol1, atol2 = atol2, rtol = rtol)
   Pval = V1'\lpseval(A1, E1, B1, F1, C1, G1, D1, H1, val)/W1
   isunimodular(A1, E1, atol1 = atol1, atol2 = atol2, rtol = rtol) ||
                error("The given linearization cannot be converted to a polynomial form")
   M1 = [A1 B1/W1; V1'\C1 V1'\D1/W1]
   N1 = [E1 F1/W1; V1'\G1 V1'\H1/W1]
   n = size(A1,1)
   indi = [1:n;[1]]
   indj = [1:n;[1]]
   n1 = n+1
   for i = 1:p
      indi[n1] = n+i
      for j = 1:m
         if abs(Pval[i,j]) > gaintol
            indj[n1] = n+j
            zer, iz, = pzeros(view(M1,indi,indj),view(N1,indi,indj);
                              fast = fast, atol1 = atol1, atol2 = atol2, rtol = rtol)
            c, pval = polcoeffval(zer[1:(length(zer)-sum(iz))],val)
            P[i,j,1:length(c)] = compl ? c*(Pval[i,j]/pval) : real(c*(Pval[i,j]/pval))
         end
      end
   end
   return P[:,:,1:pmdeg(P)+1]
end
"""
     spm2ls(T, U, V, W; fast = true, contr = false, obs = false, minimal = false, atol = 0, rtol) -> (A, E, B, C, D)

Build a structured linearization as a system matrix `S(λ)` of the form

            | A-λE | B |
     S(λ) = |------|---|
            |  C   | D |

of the structured polynomial matrix

              | -T(λ) | U(λ) |
       P(λ) = |-------|------|
              | V(λ)  | W(λ) |

such that `V(λ)*inv(T(λ))*U(λ)+W(λ) = C*inv(λE-A)*B+D`. The resulting linearization `S(λ)` preserves a part,
if `minimal = false`, or the complete Kronecker structure, if `minimal = true`, of `P(λ)`. In the latter case,
the order `n` of `A-λE` is the least possible one and `S(λ)` is a strong linearization of `P(λ)`.

`T(λ)`, `U(λ)`, `V(λ)`, and `W(λ)` can be specified as polynomial matrices of the form `X(λ) = X_1 + λ X_2 + ... + λ**k X_(k+1)`,
for `X = T`, `U`, `V`, and `W`, for which the coefficient matrices `X_i`, `i = 1, ..., k+1`, are stored in
the 3-dimensional matrices `X`, where `X[:,:,i]` contains the `i`-th coefficient matrix `X_i` (multiplying `λ**(i-1)`).

`T(λ)`, `U(λ)`, `V(λ)`, and `W(λ)` can also be specified as matrices, vectors or scalars of elements of the `Polynomial` type
provided by the [Polynomials](https://github.com/JuliaMath/Polynomials.jl) package.

The computed structured linearization satisfies:

(1) `A-λE` is regular;

(2) `rank[B A-λE] = n` (controllability) if `minimal = true` or `contr = true`, in which case
the finite and right Kronecker structures are preserved;

(3) `rank[A-λE; C] = n` (observability)  if `minimal = true` or `obs = true`, in which case
the finite and left Kronecker structures are preserved;

(4) `A-λE` has no simple infinite eigenvalues if `minimal = true`, in which case the complete Kronecker structure is preserved.

The keyword arguments `atol` and `rtol`, specify, respectively, the absolute and relative tolerance for the
nonzero coefficients of the matrices `T(λ)`, `U(λ)`, `V(λ)` and `W(λ)`. The default relative tolerance is `nt*ϵ`,
where `nt` is the size of the square matrix `T(λ)` and `ϵ` is the machine epsilon of the element type of its coefficients.

The structured linearization is built using the methods described in [1].

[1] A. Varga, On computing the Kronecker structure of polynomial and rational matrices using Julia, 2020,
[arXiv:2006.06825](https://arxiv.org/pdf/2006.06825).
"""
function spm2ls(T::Union{AbstractArray{T1,3},AbstractArray{T1,2}},U::Union{AbstractArray{T2,3},AbstractArray{T2,2}},
                V::Union{AbstractArray{T3,3},AbstractArray{T3,2}},W::Union{AbstractArray{T4,3},AbstractArray{T4,2}};
                contr::Bool = false, obs::Bool = false, minimal::Bool = false,
                fast::Bool = true, atol::Real = zero(real(T1)),
                rtol::Real = size(T,1)*eps(real(float(one(T1))))*iszero(atol)) where {T1, T2, T3, T4}
   if ndims(T) == 2
      n, nt = size(T)
      n == nt || throw(DimensionMismatch("T(λ) must be a square polynomial matrix"))
      n == rank(T, atol=atol, rtol=rtol) || error("T(λ) must be a regular square polynomial matrix")
      ndT = 1
      T = reshape(T,n,nt,ndT)
   else
      n, nt, ndT = size(T)
      n == nt || throw(DimensionMismatch("T(λ) must be a square polynomial matrix"))
      n == pmrank(T, atol=atol, rtol=rtol) || error("T(λ) must be a regular square polynomial matrix")
      ndT = max(pmdeg(T)+1,1)
   end
   if ndims(U) == 2
      nt, m = size(U)
      ndU = 1
      U = reshape(U,nt,m,ndU)
   else
      nt, m, ndU = size(U)
      ndU = max(pmdeg(U)+1,1)
   end
   n == nt || throw(DimensionMismatch("T(λ) and U(λ) must have the same number of rows"))
   if ndims(V) == 2
      p, nt = size(V)
      ndV = 1
      V = reshape(V,p,nt,ndV)
   else
      p, nt, ndV = size(V)
      ndV = max(pmdeg(V)+1,1)
   end
   n == nt || throw(DimensionMismatch("T(λ) and V(λ) must have the same number of columns"))
   if ndims(W) == 2
      pt, mt = size(W)
      ndW = 1
      W = reshape(W,pt, mt,ndW)
   else
      pt, mt, ndW = size(W)
      ndW = max(pmdeg(W)+1,1)
   end
   pt == p || throw(DimensionMismatch("W(λ) and V(λ) must have the same number of rows"))
   mt == m || throw(DimensionMismatch("W(λ) and U(λ) must have the same number of columns"))
   nd = max(ndT,ndU,ndV,ndW)
   TT = promote_type(T1, T2, T3, T4)
   if nd == 1
      if minimal
         Ar,Er,Br,Cr,Dr = lsminreal(T[:,:,1], zeros(TT,n,n), U[:,:,1], V[:,:,1], W[:,:,1],
                                    contr = false, obs = false, atol1 = atol, atol2 = atol, rtol = rtol)
         return Ar,Er,Br,Cr,Dr
      else
        return T[:,:,1], zeros(TT,n,n), U[:,:,1], V[:,:,1], W[:,:,1]
      end
   end

   # build the compound polynomial matrix [-T(λ) U(λ); V(λ) W(λ)]
   P = zeros(TT,n+p,n+m,nd)
   ia = 1:n
   jb = n+1:n+m
   ic = n+1:n+p
   P[ia,ia,1:ndT] = -T[:,:,1:ndT]
   P[ia,jb,1:ndU] = U[:,:,1:ndU]
   P[ic,ia,1:ndV] = V[:,:,1:ndV]
   P[ic,jb,1:ndW] = W[:,:,1:ndW]

   # build a linearization of the compound polynomial matrix [-T(λ) U(λ); V(λ) W(λ)]
   A, E, B, C, D = pm2ls(P, contr = contr, obs = obs, noseig = false,
                           fast = fast, atol = atol, rtol = rtol)

   # form the linearization of P(λ) = V(λ)*inv(T(λ))*U(λ)+W(λ)
   nr = size(A,1)
   Ar = [A B[:,ia]; C[ia,:] D[ia,ia]]
   Er = [E zeros(TT,nr,n); zeros(TT,n,n+nr)]
   Br = [B[:,jb]; D[ia,jb]]
   Cr = [C[ic,:] D[ic,ia]]
   Dr = D[ic,jb]
   if minimal
      Ar,Er,Br,Cr,Dr = lsminreal(Ar,Er,Br,Cr,Dr, fast=fast, atol1 = atol, atol2 = atol, rtol = rtol)
   end
   return Ar, Er, Br, Cr, Dr
end
spm2ls(T::Union{AbstractVecOrMat{Polynomial{T1}},Polynomial{T1},Number,AbstractMatrix{T1}}, U::Union{AbstractVecOrMat{Polynomial{T2}},Polynomial{T2},Number,AbstractVecOrMat{T2}},
       V::Union{AbstractVecOrMat{Polynomial{T3}},Polynomial{T3},Number,AbstractVecOrMat{T3}}, W::Union{AbstractVecOrMat{Polynomial{T4}},Polynomial{T4},Number,AbstractVecOrMat{T4}}; kwargs...) where {T1, T2, T3, T4} =
       spm2ls(poly2pm(T),poly2pm(U),poly2pm(V),poly2pm(W); kwargs...)
"""
     spm2lps(T, U, V, W; fast = true, contr = false, obs = false, minimal = false, atol = 0, rtol) -> (A, E, B, F, C, G, D, H)

Build a structured linearization

              | A-λE | B-λF |
     M - λN = |------|------|
              | C-λG | D-λH |

of the structured polynomial matrix

              | -T(λ) | U(λ) |
       P(λ) = |-------|------|
              |  V(λ) | W(λ) |

such that `V(λ)*inv(T(λ))*U(λ)+W(λ) = (C-λG))*inv(λE-A)*(B-λF)+D-λH`. The resulting linearization `M - λN` preserves a part,
if `minimal = false`, or the complete Kronecker structure, if `minimal = true`, of `P(λ)`. In the latter case,
the order `n` of `A-λE` is the least possible one and `M - λN` is a strong linearization of `P(λ)`.

`T(λ)`, `U(λ)`, `V(λ)`, and `W(λ)` can be specified as polynomial matrices of the form `X(λ) = X_1 + λ X_2 + ... + λ**k X_(k+1)`,
for `X = T`, `U`, `V`, and `W`, for which the coefficient matrices `X_i`, `i = 1, ..., k+1`, are stored in
the 3-dimensional matrices `X`, where `X[:,:,i]` contains the `i`-th coefficient matrix `X_i` (multiplying `λ**(i-1)`).

`T(λ)`, `U(λ)`, `V(λ)`, and `W(λ)` can also be specified as matrices, vectors or scalars of elements of the `Polynomial` type
provided by the [Polynomials](https://github.com/JuliaMath/Polynomials.jl) package.

The computed structured linearization satisfies:

(1) `A-λE` is regular;

(2) `rank[B-λF A-λE] = n` (strong controllability) if `minimal = true` or `contr = true`, in which case
the finite and right Kronecker structures are preserved;

(3) `rank[A-λE; C-λG] = n` (strong observability)  if `minimal = true` or `obs = true`, in which case
the finite and left Kronecker structures are preserved.

The keyword arguments `atol` and `rtol`, specify, respectively, the absolute and relative tolerance for the
nonzero coefficients of the matrices `T(λ)`, `U(λ)`, `V(λ)` and `W(λ)`. The default relative tolerance is `nt*ϵ`,
where `nt` is the size of the square matrix `T(λ)` and `ϵ` is the machine epsilon of the element type of its coefficients.
"""
function spm2lps(T::Union{AbstractArray{T1,3},AbstractArray{T1,2}},U::Union{AbstractArray{T2,3},AbstractArray{T2,2}},
                V::Union{AbstractArray{T3,3},AbstractArray{T3,2}},W::Union{AbstractArray{T4,3},AbstractArray{T4,2}};
                contr::Bool = false, obs::Bool = false, minimal::Bool = false,
                fast::Bool = true, atol::Real = zero(real(T1)),
                rtol::Real = size(T,1)*eps(real(float(one(T1))))*iszero(atol)) where {T1, T2, T3, T4}
   if ndims(T) == 2
      n, nt = size(T)
      n == nt || throw(DimensionMismatch("T(λ) must be a square polynomial matrix"))
      n == rank(T, atol=atol, rtol=rtol) || error("T(λ) must be a regular square polynomial matrix")
      ndT = 1
      T = reshape(T,n,nt,ndT)
   else
      n, nt, ndT = size(T)
      n == nt || throw(DimensionMismatch("T(λ) must be a square polynomial matrix"))
      n == pmrank(T, atol=atol, rtol=rtol) || error("T(λ) must be a regular square polynomial matrix")
      ndT = max(pmdeg(T)+1,1)
   end
   if ndims(U) == 2
      nt, m = size(U)
      ndU = 1
      U = reshape(U,nt,m,ndU)
   else
      nt, m, ndU = size(U)
      ndU = max(pmdeg(U)+1,1)
   end
   n == nt || throw(DimensionMismatch("T(λ) and U(λ) must have the same number of rows"))
   if ndims(V) == 2
      p, nt = size(V)
      ndV = 1
      V = reshape(V,p,nt,ndV)
   else
      p, nt, ndV = size(V)
      ndV = max(pmdeg(V)+1,1)
   end
   n == nt || throw(DimensionMismatch("T(λ) and V(λ) must have the same number of columns"))
   if ndims(W) == 2
      pt, mt = size(W)
      ndW = 1
      W = reshape(W,pt, mt,ndW)
   else
      pt, mt, ndW = size(W)
      ndW = max(pmdeg(W)+1,1)
   end
   pt == p || throw(DimensionMismatch("W(λ) and V(λ) must have the same number of rows"))
   mt == m || throw(DimensionMismatch("W(λ) and U(λ) must have the same number of columns"))
   nd = max(ndT,ndU,ndV,ndW)
   TT = promote_type(T1, T2, T3, T4)
   if nd == 1
      if minimal
         Ar,Er,Br,Cr,Dr = lsminreal(T[:,:,1], zeros(TT,n,n), U[:,:,1], V[:,:,1], W[:,:,1],
                                    contr = false, obs = false, atol1 = atol, atol2 = atol, rtol = rtol)
         nr = size(Ar,1)
         TW = eltype(Ar)
         return Ar,Er,Br,zeros(TW,nr,m),Cr,zeros(TW,p,nr),Dr,zeros(TW,p,m)
      else
        return T[:,:,1], zeros(TT,n,n), U[:,:,1], zeros(TT,n,m), V[:,:,1], zeros(TT,p,n), W[:,:,1], zeros(TT,p,m)
      end
   end

   # build the compound polynomial matrix [-T(λ) U(λ); V(λ) W(λ)]
   P = zeros(TT,n+p,n+m,nd)
   ia = 1:n
   jb = n+1:n+m
   ic = n+1:n+p
   P[ia,ia,1:ndT] = -T[:,:,1:ndT]
   P[ia,jb,1:ndU] = U[:,:,1:ndU]
   P[ic,ia,1:ndV] = V[:,:,1:ndV]
   P[ic,jb,1:ndW] = W[:,:,1:ndW]

   # build a linearization of the compound polynomial matrix [-T(λ) U(λ); V(λ) W(λ)]
   A, E, B, F, C, G, D, H = pm2lps(P, contr = contr, obs = obs)

   # form the linearization of P(λ) = V(λ)*inv(T(λ))*U(λ)+W(λ)
   nr = size(A,1)
   Ar = [A B[:,ia]; C[ia,:] D[ia,ia]]
   Er = [E F[:,ia]; G[ia,:] H[ia,ia]]
   Br = [B[:,jb]; D[ia,jb]]
   Fr = [F[:,jb]; H[ia,jb]]
   Cr = [C[ic,:] D[ic,ia]]
   Gr = [G[ic,:] H[ic,ia]]
   Dr = D[ic,jb]
   Hr = H[ic,jb]
   if minimal
      Ar,Er,Br,Fr,Cr,Gr,Dr,Hr,Vr,Wr = lpsminreal(Ar,Er,Br,Fr,Cr,Gr,Dr,Hr, fast=fast, atol1 = atol, atol2 = atol, rtol = rtol)
      return Ar,Er,Br/Wr,Fr/Wr,Vr'\Cr,Vr'\Gr,Vr'\Dr/Wr,Vr'\Hr/Wr
   else
      return Ar,Er,Br,Fr,Cr,Gr,Dr,Hr
   end
end
function spm2lps(T::Union{AbstractVecOrMat{<:Polynomial},Polynomial,Number,AbstractVecOrMat{<:Number}},
        U::Union{AbstractVecOrMat{<:Polynomial},Polynomial,Number,AbstractVecOrMat{<:Number}},
        V::Union{AbstractVecOrMat{<:Polynomial},Polynomial,Number,AbstractVecOrMat{<:Number}},
        W::Union{AbstractVecOrMat{<:Polynomial},Polynomial,Number,AbstractVecOrMat{<:Number}}; kwargs...)
   # TODO: checking that all entries have the same variable
    return spm2lps(poly2pm(T),poly2pm(U),poly2pm(V),poly2pm(W); kwargs...)
end


function random_walk(steps::Int64)::Array{Float64, 1}
    SQRT_STEPS = sqrt(steps)
    walk = ones(steps)
    for idx in 2:steps
        if rand() < .5
            y = -1
        else
            y = 1
        end
        walk[idx] = walk[idx-1] + (y / SQRT_STEPS)
    end
    walk
end

random_walk(100)

multiple = 1
for i in 1:8
    global multiple
    t = @elapsed random_walk(multiple)
    println("jl_random_walk\t$t")
    multiple *= 10
end


"""
    peakflops_gpu_matmul_scaling(peakflops_func = peakflops_gpu_matmul; verbose=true) -> sizes, flops
Asserts the scaling of the given `peakflops_func`tion (defaults to [`peakflops_gpu_matmul`](@ref))
with increasing matrix size. If `verbose=true` (default), displays a unicode plot. Returns
the considered sizes and TFLOP/s. For further options, see [`peakflops_gpu_matmul`](@ref).
"""
function peakflops_gpu_matmul_scaling(
    peakflops_func::F=peakflops_gpu_matmul;
    device=CUDA.device(),
    verbose=true,
    sizes=2 .^ (10:15),
    io::IO=stdout,
    kwargs...,
) where {F}
    flops = zeros(length(sizes))
    for (i, s) in enumerate(sizes)
        flops[i] = peakflops_func(; device=device, size=s, verbose=false, kwargs...)
        clear_gpu_memory(device)
    end
    if verbose
        peak_val, idx = findmax(flops)
        peak_size = sizes[idx]
        p = UnicodePlots.lineplot(
            sizes,
            flops;
            xlabel="matrix size",
            ylabel="TFLOP/s",
            title=string(
                "Peak: ", round(peak_val; digits=2), " TFLOP/s (size = $(peak_size))"
            ),
            xscale=:log2,
        )
        UnicodePlots.lineplot!(p, [peak_size, peak_size], [0.0, peak_val]; color=:red)
        println(io) # top margin
        show(io, "text/plain",p)
        println(io) # bottom margin
        println(io) # bottom margin
    end
    return sizes, flops
end

_flopcount_per_matmul(n) = Float64(n)^3

"""
    peakflops_gpu_matmul(; device, dtype=Float32, size=2^14, nmatmuls=5, nbench=5, verbose=true)
Tries to estimate the peak performance of a GPU in TFLOP/s by measuring the time
it takes to perform `nmatmuls` many (in-place) matrix-matrix multiplications.

**Keyword arguments:**
* `device` (default: `CUDA.device()`): CUDA device to be used.
* `dtype` (default: `Float32`): element type of the matrices.
* `size` (default: `2^14`): matrices will have dimensions `(size, size)`.
* `nmatmuls` (default: `5`): number of matmuls that will make up the kernel to be timed.
* `nbench` (default: `5`): number of measurements to be performed the best of which is used for the TFLOP/s computation.
* `verbose` (default: `true`): toggle printing.
* `io` (default: `stdout`): set the stream where the results should be printed.

See also: [`peakflops_gpu_matmul_scaling`](@ref), [`peakflops_gpu_matmul_graphs`](@ref).
"""
function peakflops_gpu_matmul(;
    device=CUDA.device(), dtype=Float32, size=2^14, nmatmuls=5, nbench=5, verbose=true, io::IO=stdout
)
    device!(device) do
        C = CUDA.zeros(dtype, size, size)
        A = CUDA.rand(dtype, size, size)
        B = CUDA.rand(dtype, size, size)

        CUDA.@elapsed mul!(C, A, B) # warmup

        t = Inf
        NVTX.@range "peakflops_gpu: bench loop" begin
            for i in 1:nbench
                NVTX.@range "peakflops_gpu: kernel" begin
                    Δt = CUDA.@elapsed for _ in 1:nmatmuls
                        mul!(C, A, B)
                        # cublasGemmEx_wrapper!('N','N',A,B,C)
                    end
                end
                t = min(t, Δt)
            end
        end

        flops = (_flopcount_per_matmul(size) * nmatmuls * 1e-12) / t
        if verbose
            printstyled(io,"Peakflops (TFLOP/s):\n"; bold=true)
            print(io," └ max: ")
            printstyled(io,round(flops; digits=2), "\n"; color=:green, bold=true)
        end
        return flops
    end
end

"""
Same as [`peakflops_gpu_matmul`](@ref) but uses CUDA's graph API to define and launch the kernel.

See also: [`peakflops_gpu_matmul_scaling`](@ref).
"""
function peakflops_gpu_matmul_graphs(;
    device=CUDA.device(), dtype=Float32, size=2^14, nmatmuls=5, nbench=5, verbose=true, io::IO=stdout
)
    device!(device) do
        C = CUDA.zeros(dtype, size, size)
        A = CUDA.rand(dtype, size, size)
        B = CUDA.rand(dtype, size, size)

        t = Inf
        for i in 1:nbench
            graph = CUDA.capture() do
                for _ in 1:nmatmuls
                    mul!(C, A, B)
                    # cublasGemmEx_wrapper!('N','N',A,B,C)
                end
            end
            exec = instantiate(graph)
            Δt = CUDA.@elapsed CUDA.launch(exec)
            t = min(t, Δt)
        end

        flops = (_flopcount_per_matmul(size) * nmatmuls * 1e-12) / t
        if verbose
            printstyled(io,"Peakflops (TFLOP/s):\n"; bold=true)
            print(io," └ max: ")
            printstyled(io,round(flops; digits=2), "\n"; color=:green, bold=true)
        end
        return flops
    end
end


# ------------------------------------------------------------------------------------------
# # Metaprogramming with Julia
# ------------------------------------------------------------------------------------------

# ------------------------------------------------------------------------------------------
# ## Scare Quotes
# ------------------------------------------------------------------------------------------

# ------------------------------------------------------------------------------------------
# Julia supports *metaprogramming*. This is similar to symbolic programming, where we deal
# with expressions (like $2+2$) as opposed to values (like $4$).
#
# Normally, Julia takes all code we give it as a set of instructions, and carries them out.
# If we type `2+2` it will faithfully add those numbers and give us the result.
# ------------------------------------------------------------------------------------------

2+2

# ------------------------------------------------------------------------------------------
# We can prevent this from happening with quotation marks. Surrounding our code with `"`
# treats it as a literal string of characters, without seeing it as code at all.
# ------------------------------------------------------------------------------------------

x = "2+2"

# ------------------------------------------------------------------------------------------
# We can then explicitly tell Julia to evaluate it later on.
# ------------------------------------------------------------------------------------------

eval(Meta.parse(ans))

# ------------------------------------------------------------------------------------------
# Why go through this complicated way to add numbers? The trick is that when we have the
# expression `2+2`, we can modify it in various interesting ways. As a simple example,
# imagine replacing `+` with `-`.
# ------------------------------------------------------------------------------------------

x = replace(x, "+"=>"-")

eval(Meta.parse(x))

# ------------------------------------------------------------------------------------------
# We don't actually want to work with strings here; Julia has a much more powerful way to
# quote code, the frowny face operator `:()`.
# ------------------------------------------------------------------------------------------

x = :(2+2)

eval(x)

# ------------------------------------------------------------------------------------------
# We can quote larger expression, including blocks and entire function definitions. The
# keyword `quote` is an alternative to `begin` that returns the quoted block.
#
# In larger blocks, Julia will preserve line number information, which appears as a comment.
# ------------------------------------------------------------------------------------------

quote
  x = 2 + 2
  hypot(x, 5)
end

:(function mysum(xs)
    sum = 0
    for x in xs
      sum += x
    end
  end)

# ------------------------------------------------------------------------------------------
# ## Fruit of the Expression Tree
# ------------------------------------------------------------------------------------------

# ------------------------------------------------------------------------------------------
# Strings support "interpolation", which allows us to easily build larger strings from
# smaller components.
# ------------------------------------------------------------------------------------------

x = "yields falsehood when preceded by its quotation"
println(x)

y = "'$x' $x"
println(y)

x = :(2+2)

:($x * $x)

eval(ans)

# ------------------------------------------------------------------------------------------
# ## The Root of all Eval
# ------------------------------------------------------------------------------------------

# ------------------------------------------------------------------------------------------
# `eval` can do more than just returning a result. What happens if we quote something like a
# function definition?
# ------------------------------------------------------------------------------------------

ex = :(foo() = println("I'm foo!"))

# ------------------------------------------------------------------------------------------
# It doesn't actually do anything; yet.
# ------------------------------------------------------------------------------------------

foo()  # throws UndefVarError

# ------------------------------------------------------------------------------------------
# But evaluating `ex` brings `foo` to life!
# ------------------------------------------------------------------------------------------

eval(ex)

foo()

# ------------------------------------------------------------------------------------------
# Using interpolation, we can construct a function definition on-the-fly; in fact, we can
# make a whole series of functions at once.
# ------------------------------------------------------------------------------------------

for name in [:foo, :bar, :baz]
  println(:($name() = println($("I'm $(name)!"))))
end

# ------------------------------------------------------------------------------------------
# And then bring them to life with `eval`, too.
# ------------------------------------------------------------------------------------------

for name in [:foo, :bar, :baz]
  eval(:($name() = println($("I'm $(name)!"))))
end

bar()

baz()

# ------------------------------------------------------------------------------------------
# This can be an *extremely* useful trick when wrapping APIs (say, from a C library or over
# HTTP). APIs often define a list of available functions, so you can grab that and generate
# the whole wrapper at once! See Clang.jl, TensorFlow.jl, or the Base linear algebra
# wrappers for examples.
# ------------------------------------------------------------------------------------------

# ------------------------------------------------------------------------------------------
# ## Original sin
# ------------------------------------------------------------------------------------------

# ------------------------------------------------------------------------------------------
# Here's a more practical example. Consider the following definition of the `sin` function,
# based on the Taylor series.
#
# $$sin(x) = \sum_{k=1}^{\infty} \frac{(-1)^k}{(1+2k)!} x^{1+2k}$$
# ------------------------------------------------------------------------------------------

mysin(x) = sum((-1)^k/factorial(1+2k) * x^(1+2k) for k = 0:5)

mysin(0.5), sin(0.5)

# ------------------------------------------------------------------------------------------
# To see where we are right now, we'll benchmark it.
# ------------------------------------------------------------------------------------------

using BenchmarkTools
@benchmark mysin(0.5)

# ------------------------------------------------------------------------------------------
# Right now, this is much slower than it could be. The reason is that we're looping over
# `k`, which is relatively expensive. It'd be much faster to write out:
# ------------------------------------------------------------------------------------------

mysin(x) = x - x^3/6 + x^5/120 # + ...

# ------------------------------------------------------------------------------------------
# But this is tedious to write, and no longer looks like the original Taylor series. It's
# harder to tell if we've made a mistake, and we easily modify it. Is there a way to get the
# best of both worlds?
#
# How about getting Julia to write out that code for us?
#
# To start with, let's consider a symbolic version of the `+` function.
# ------------------------------------------------------------------------------------------

plus(a, b) = :($a + $b)

plus(1, 2)

# ------------------------------------------------------------------------------------------
# With `plus` we can do more interesting things, like symbolic `sum`:
# ------------------------------------------------------------------------------------------

reduce(+, 1:10)

reduce(plus, 1:10)

eval(ans)

# ------------------------------------------------------------------------------------------
# Given that, we can also sum over symbolic variables.
# ------------------------------------------------------------------------------------------

reduce(plus, [:(x^2), :x, 1])

# ------------------------------------------------------------------------------------------
# This gives us an important piece of the puzzle, but we also need to figure out _what_
# we're summing. Let's crate a symbolic version of the Taylor series above, which
# interpolates the value of `k`.
# ------------------------------------------------------------------------------------------

k = 2
:($((-1)^k) * x^$(1+2k) / $(factorial(1+2k)))

# ------------------------------------------------------------------------------------------
# Now we have one term, we can generate as many as we like.
# ------------------------------------------------------------------------------------------

terms = [:($((-1)^k) * x^$(1+2k) / $(factorial(1+2k))) for k = 0:5]

# ------------------------------------------------------------------------------------------
# And sum them –
# ------------------------------------------------------------------------------------------

reduce(plus, ans)

# ------------------------------------------------------------------------------------------
# And create a function definition out of it:
# ------------------------------------------------------------------------------------------

:(mysin(x) = $ans)

eval(ans)

mysin(0.5), sin(0.5)

@benchmark mysin(0.5)

# ------------------------------------------------------------------------------------------
# On my machine `sin2` takes about 50 *nano*seconds to run – not bad for a naive
# implementation. If we challenged a photon to a twenty metre sprint, we'd win!
# ------------------------------------------------------------------------------------------

# ------------------------------------------------------------------------------------------
# ## Beneath the Expression
# ------------------------------------------------------------------------------------------

# ------------------------------------------------------------------------------------------
# It's really just a normal tree data structure, and we can take a look inside it.
# ------------------------------------------------------------------------------------------

ex = :(f(x, y))
ex = Expr(:call, :f, :x, :y)
@show ex.head;
@show ex.args;

# ------------------------------------------------------------------------------------------
# In our example above, we replace `+` with `-` in a string. In an expression, we can do
# that by altering the `.args` of the expression.
# ------------------------------------------------------------------------------------------

ex = :(2+2)

ex.args[1] = :-
ex

eval(ex)

# ------------------------------------------------------------------------------------------
# Note that larger, more complex expressions are a bit trickier than this. They are
# *nested*, which means that the expression `2+3` is contained inside the larger expression
# `1 + (2 + 3)`.
# ------------------------------------------------------------------------------------------

ex = :(1 + (2 + 3))

ex.args

# ------------------------------------------------------------------------------------------
# A package called MacroTools provides a way to deal with this. It shows us *all* sub-
# expressions in turn, allowing us to decide how we want to change them. It can be thought
# of as a bit like a find-and-replace operation. Here's an example that finds all integers
# in an expression, and increments them.
# ------------------------------------------------------------------------------------------

# import Pkg; Pkg.add("MacroTools")
using MacroTools
using MacroTools: postwalk

postwalk(ex) do x
  x isa Integer ? x+1 : x
end

# ------------------------------------------------------------------------------------------
# To get a feel for what's happening, you can use `@show` to see what `postwalk` sees.
#
# (`@show` is Julia's single most useful feature; if you don't understand what code is
# doing, throw it in and see what's happening.)
# ------------------------------------------------------------------------------------------

map(x -> @show(x), [1,2,3])

postwalk(ex) do x
  @show x
end

# ------------------------------------------------------------------------------------------
# MacroTools also provides tools for *pattern matching* over expressions. `a_ + b_` acts as
# a template; if the expression provided look like the template, `a` and `b` will match the
# two things be added. If not, they'll just be `nothing`.
# ------------------------------------------------------------------------------------------

ex = :(2 + 3)
@capture(ex, a_ + b_)

a, b

ex = :(f(2,3))
@capture(ex, a_ + b_)

a, b

# ------------------------------------------------------------------------------------------
# We can finally use this to replace _all_ `+`s with `-`s in an expression, rather than just
# one.
# ------------------------------------------------------------------------------------------

ex = :(3x^2 + (2x + 1))

postwalk(ex) do x
  @capture(x, a_ + b_) || return x
  :($a - $b)
end

# ------------------------------------------------------------------------------------------
# ## Macro Agressions
# ------------------------------------------------------------------------------------------

# ------------------------------------------------------------------------------------------
# You have probably seen macros already – basic features in Julia like `@show`, `@time` and
# `@inline` are really macros. For basic usage, it's enough to think of them as simple
# annotations that alter how code is run. But we now know enough to dig into how they work
# under the hood.
#
# Normal functions never see _code_, only _values_. If we pass `2+2` to a function, it sees
# `4`.
# ------------------------------------------------------------------------------------------

function foo(x)
  @show x
  return x
end

foo(2+2)

# ------------------------------------------------------------------------------------------
# Macros are much like function, but they see _code_ that was passed to them, like the
# expressions that we saw above. Macros get a chance to manipulate this code and alter how
# it behaves.
# ------------------------------------------------------------------------------------------

macro foo(x)
  @show x
  return x
end

@foo(2+2)

# ------------------------------------------------------------------------------------------
# As a simple example, we can replace one of the arguments to `+` and get a different result
# back.
# ------------------------------------------------------------------------------------------

macro foo(x)
  x.args[2] = 5
  return x
end

@foo(2+2)

# ------------------------------------------------------------------------------------------
# MacroTools provides a useful tool, `@expand`, to see what's going on inside a macro; it
# reveals the code that the macro returns without running it.
# ------------------------------------------------------------------------------------------

@expand @foo(2+2)

# ------------------------------------------------------------------------------------------
# You can of course use this on the macros that come with Julia, and it's a good way to
# learn how they work.
# ------------------------------------------------------------------------------------------

@time 2+2

@expand @time 2+2

# ------------------------------------------------------------------------------------------
# Let's do something more advanced. Using the techniques we developed above, we can find-
# and-replace `+` expressions in code. We'll still do the addition, but we'll also log what
# we added for debugging purposes.
#
# First, let's make an example expression using quotation.
# ------------------------------------------------------------------------------------------

ex = quote
  s = 0
  for x in xs
    s = s + x
  end
  return s
end

# ------------------------------------------------------------------------------------------
# We can use `ex` to develop the code transformation we want, and check that the code does
# the right thing.}
# ------------------------------------------------------------------------------------------

postwalk(ex) do x
  @capture(x, a_ + b_) || return x
  quote
    println("Adding " * string($a) * " to " * string($b))
    $x
  end
end

# ------------------------------------------------------------------------------------------
# Now we just wrap this in a macro, and we can add it to a normal function definition!
# ------------------------------------------------------------------------------------------

macro log_adds(ex)
  postwalk(ex) do x
    @capture(x, a_ + b_) || return x
    quote
      println("Adding " * string($a) * " to " * string($b))
      $x
    end
  end
end

@log_adds function mysum(xs)
  sum = 0
  for x in xs
    sum = sum + x
  end
  return sum
end

mysum(1:10)

# ------------------------------------------------------------------------------------------
# Adding and removing `@log_adds` is much nicer than inserting the debug calls by hand,
# especially if you have lots of `+`s in the code. Perhaps you can extend the macro to
# support logging other operators, like `-`.
# ------------------------------------------------------------------------------------------

# ------------------------------------------------------------------------------------------
# ## Generated Functions
# ------------------------------------------------------------------------------------------

# ------------------------------------------------------------------------------------------
# _Generated functions_ are a new metaprogramming tool unique to Julia. This section will
# briefly describe and motivate them, and [this blog
# post](http://mikeinnes.github.io/2017/08/24/cudanative.html) gives more examples for the
# interested reader.
#
# Essentially, a generated function is like a macro that operates on _types_ rather than
# expression trees. If we try to inspect arguments, we'll see their types rather than their
# values.
# ------------------------------------------------------------------------------------------

@generated function gadd(a, b)
  Core.println("a = $a, b = $b")
  :(a+b)
end

gadd(5, 2.5)

# ------------------------------------------------------------------------------------------
# Generated functions become more powerful when working with more complex types. For
# example, notice how arrays store their rank inside the type, so this is something we can
# generate code for.
# ------------------------------------------------------------------------------------------

rand(2,2)

typeof(ans)

# ------------------------------------------------------------------------------------------
# Why might this be useful? If you write code to deal with arrays, you'll notice that you
# often need a `for` loop for each dimension of the array. If you want to work with a 7D
# array, you need to write seven nested loops!
# ------------------------------------------------------------------------------------------

function mysum(xs::Array{<:Any,1})
  sum = 0
  for i = 1:length(xs)
    sum += xs[i]
  end
  return sum
end

function mysum(xs::Array{<:Any,2})
  sum = 0
  for i = 1:size(xs,1)
    for j = 1:size(xs, 2)
      sum += xs[i]
    end
  end
  return sum
end

# ------------------------------------------------------------------------------------------
# Where other languages simply hard-code a version of each function for vectors, matrices
# and perhaps 3D arrays, Julia allows us to easily write N-dimensional algorithms by
# generating nested loops as needed.
# ------------------------------------------------------------------------------------------


module mbox1
using MeshSteward: initbox, updatebox!, inflatebox!, inbox, boxesoverlap
using Test
function test()
	box = Float64[]
	box = initbox([0.0])
	@test isapprox(box, [0.0, 0.0])

	box = updatebox!(box, [-1.0])
	@test isapprox(box, [-1.0, 0.0])

	box = Float64[]
	box = initbox([1.0])
	inflate = 0.01
	box = inflatebox!(box, inflate)
	@test isapprox(box, [0.99, 1.01])

	box = initbox([0.0])
	inflate = 0.01
	box = inflatebox!(box, inflate)
	@test inbox(box, [-0.003]) == true

	box = initbox([2.0])
	updatebox!(box, [1.0])
	inflate = 0.01
	box = inflatebox!(box, inflate)
	@test inbox(box, [rand()+1.0]) == true

	box = Float64[]
	box = initbox([0.0; 1.0])
	@test isapprox(box, [0.0, 0.0, 1.0, 1.0])

	box = updatebox!(box, [1.0; -1.0])
	@test isapprox(box, [0.0, 1.0, -1.0, 1.0])

	box = Float64[]
	box = initbox([0.0; 1.0])
	inflate = 0.01
	box = inflatebox!(box, inflate)
	@test isapprox(box, [-0.01, 0.01, 0.99, 1.01])

	box = initbox([0.0; 1.0])
	inflate = 0.01
	box = inflatebox!(box, inflate)
	@test inbox(box, [-0.003; 0.995]) == true

	box = initbox([0.0; 0.0])
	updatebox!(box, [1.0; 1.0])
	inflate = 0.01
	box = inflatebox!(box, inflate)
	@test inbox(box, [-0.003; 0.995]) == true
	@test inbox(box, [rand(); rand()]) == true
	@test inbox(box, [0.5; 0.5]) == true
	@test inbox(box, [0.0-inflate; 0.0-inflate]) == true
	@test inbox(box, [0.0-inflate; 0.0-2*inflate]) == false
	@test inbox(box, [0.0+inflate; 1.0+inflate]) == true

	box = initbox([0.0; 0.0; 0.0].-1.0)
	updatebox!(box, [1.0; 1.0; 1.0])
	inflate = 0.01
	box = inflatebox!(box, inflate)
	@test inbox(box, [-0.003; 0.995; 1.0]) == true
	@test inbox(box, [rand(); rand(); rand()]) == true
	@test inbox(box, [-rand(); rand(); rand()]) == true
	@test inbox(box, [rand(); -rand(); rand()]) == true
	@test inbox(box, [rand(); rand(); -rand()]) == true
	@test inbox(box, [0.0-inflate; 0.0-inflate; 1.0]) == true
	@test inbox(box, [0.0-inflate; 0.0-inflate; -1.0]) == true

	return true
end
end
using .mbox1
mbox1.test()

module mbox2
using MeshSteward: initbox, updatebox!, inflatebox!, inbox, boxesoverlap, boundingbox
using LinearAlgebra
using Test
function test()
	a = [0.431345 0.611088 0.913161;
	    0.913581 0.459229 0.82186;
	    0.999429 0.965389 0.571139;
	    0.390146 0.711732 0.302495;
	    0.873037 0.248077 0.51149;
	    0.999928 0.832524 0.93455]
	    b1 = boundingbox(a)
	    @test norm(b1 - [0.390146, 0.999928, 0.248077, 0.965389, 0.302495, 0.93455]) < 1.0e-4
	    b2 = updatebox!(b1, a)
	    @test norm(b1 - b2) < 1.0e-4
	    b2 = initbox(a)
	    @test norm(b1 - b2) < 1.0e-4
	    c = [-1.0, 3.0, -0.5]
	    b3 = updatebox!(b1, c)
	    # # println("$(b3)")
	    @test norm(b3 - [-1.0, 0.999928, 0.248077, 3.0, -0.5, 0.93455]) < 1.0e-4
	    x = [0.25 1.1 -0.3]
	    @test inbox(b3, x)
	    @test inbox(b3, c)
	    @test inbox(b3, a[2, :])
	    b4 = boundingbox(-a)
	    # # println("$(b3)")
	    # # println("$(b4)")
	    # # println("$(boxesoverlap(b3, b4))")
	    @test !boxesoverlap(b3, b4)
	    b5 = updatebox!(b3, [0.0 -0.4 0.0])
	    # # println("$(b5)")
	    # # println("$(boxesoverlap(b5, b4))")
	    @test boxesoverlap(b5, b4)
end
end
using .mbox2
mbox2.test()

module mboxt3
using MeshSteward: intersectboxes
using LinearAlgebra
using Test
function test()
	b1 = [-1.0, 2.0, 0.5, 2.5]
	b2 = [-0.7, 1.2, -0.5, 3.5]
	ib = intersectboxes(b1, b2)
	@test isapprox(ib, [-0.7, 1.2, 0.5, 2.5])
	b1 = [-1.0, 2.0, 0.5, 2.5, 77.1, 1000.0]
	b2 = [-0.7, 1.2, -0.5, 3.5, 1077.1, 10000.0]
	ib = intersectboxes(b1, b2)
	@test isapprox(ib, Float64[])
end
end
using .mboxt3
mboxt3.test()


import Base.+, Base.-, Base./, Base.*, Base.^

# PE_Units
function change_base_(u::PE_Unit, new_base::Float64) # Intentially changing name so this is not exported.
    old_base = u.base_
    diff = new_base - old_base
    if diff  ≂ 0.0
        return Array{Tuple{Float64,PE_Unit},1}([(1.0, u)])
    end
    # First the exponential part.
    mult = exp(u.b_*diff)
    if mult ≂ 0.0
        error("Underflow problem. Changing to this base cannot be done")
    end
    # Now the polynomial part.
    if u.d_ == 0
        new_unit = PE_Unit(u.b_, new_base, 0)
        return Array{Tuple{Float64,PE_Unit},1}([(mult, new_unit)])
    else
        n = u.d_
        funcs = Array{Tuple{Float64,PE_Unit},1}(undef,n+1)
        for r in 0:n
            binom_coeff = factorial(n) / (factorial(r) * factorial(n-r))
            new_multiplier = binom_coeff * mult * diff^r
            new_unit = PE_Unit(u.b_, new_base, n-r)
            new_func = (new_multiplier, new_unit)
            funcs[r+1] = new_func
        end
        return funcs
    end
end

"""
    change_base(f::PE_Function, new_bases::Dict{Symbol,Float64})
This function changes the bases in the PE_Units of a PE_Function. This is useful for getting two
PE_Functions comformable for simpler multiplication. Often a base change means that an array of
PE_Functions are needed to represent a function. So an Array{PE_Function,1} is returned.
"""
function change_base(f::PE_Function, new_bases::Dict{Symbol,Float64})
    dims_dict = Dict{Symbol,Array{Tuple{Float64,PE_Unit},1}}()
    dims = keys(f.units_)
    for dim in dims
        if haskey(new_bases, dim)
            desired_base = new_bases[dim]
            f_unit = f.units_[dim]
            f_base = f_unit.base_
            changed_pes = change_base_(f_unit, desired_base)
            dims_dict[dim] = changed_pes
        else
            dims_dict[dim] = [(1.0,f.units_[dim])]
        end
    end
    array_of_tups = [Dict{Symbol,Tuple{Float64,PE_Unit}}(dims .=> val) for val in (collect(Iterators.product(getindex.((dims_dict,),dims)...))...,)]
    array_of_pes = PE_Function.(f.multiplier_, array_of_tups)
    return array_of_pes
end

function *(u1::PE_Unit,u2::PE_Unit)
    if u1.base_ ≂ u2.base_
        return PE_Unit(u1.b_ + u2.b_, u1.base_, u1.d_ + u2.d_)
    else
        error("Cannot multiply PE_Units without reconciling the bases first.")
    end
end

# PE_Functions
"""
   +(f::MultivariateFunction,number::Float64)
   +(f::MultivariateFunction,number::Int)

   A Multivariate Function can be added to an scalar to from a new MultivariateFunction. This action promotes a
   PE_Function to a Sum_Of_Functions. The type of all other MultivariateFunctions is unchanged.
"""
function +(f::PE_Function,number::Float64)
    constant_function = PE_Function(number, 0.0,0.0,0)
    return Sum_Of_Functions([f, constant_function])
end
"""
   -(f::MultivariateFunction,number::Float64)
   -(f::MultivariateFunction,number::Int)

   A scalar can be subtracted from a Multivariate Function. This action promotes a
   PE_Function to a Sum_Of_Functions. The type of all other MultivariateFunctions is unchanged.
"""
function -(f::PE_Function, number::Float64)
    return +(f, -number)
end
"""
   *(f::MultivariateFunction,number::Float64)
   *(f::MultivariateFunction,number::Int)

   A Multivariate can be multiplied by a scalar. This does not change the type of any MultivariateFunction.
"""
function *(f::PE_Function, number::Float64)
    return PE_Function(f.multiplier_*number, f.units_)
end
"""
   /(f::MultivariateFunction,number::Float64)
   /(f::MultivariateFunction,number::Int)

   A Multivariate can be divided by a scalar. This does not change the type of any MultivariateFunction.
   Note that the opposite operation cannot be done. While f / 5 is permitted 5 / f is not supported by this package.
   It is not possible to divide by a function.
"""
function /(f::PE_Function, number::Float64)
    return *(f, 1/number )
end

function +(f::PE_Function, number::Int)
    number_as_float = convert(Float64, number)
    return +(f, number_as_float)
end
function -(f::PE_Function, number::Int)
    number_as_float = convert(Float64, number)
    return -(f, number_as_float)
end
function *(f::PE_Function, number::Int)
    number_as_float = convert(Float64, number)
    return *(f, number_as_float)
end
function /(f::PE_Function, number::Int)
    number_as_float = convert(Float64, number)
    return /(f, number_as_float)
end
"""
   ^(f::MultivariateFunction,number::Int)

   A Multivariate can be raised by a positive Integer power. This will generally promote a PE_Function to a Sum_Of_Functions.
   Note that the opposite operation cannot be done. While f ^ 5 is permitted 5 ^ f is not supported by this package.
   It is not possible to raise by the power of a function.
"""
function ^(f::PE_Function, number::Int)
    if number < 0
        error("Negative powers are not supported.")
    elseif number == 0
        return PE_Function(1.0)
    elseif number == 1
        return f
    else
        val = f * f
        for i in 3:number
            val = val * f
        end
        return val
    end
end
function ^(f::PE_Function, number::Float64)
    error("Cannot raise a PE_Function to a float value.")
end
"""
    +(f1::MultivariateFunction, f2::MultivariateFunction)
    Any two multivariate Functions can be added to form a MultivariateFunction reflecting the sum.
"""
function +(f1::PE_Function, f2::PE_Function)
    return Sum_Of_Functions([f1,f2])
end
function +(f1::PE_Function, f2::Sum_Of_Functions)
    return Sum_Of_Functions([f1,f2])
end
function +(f1::PE_Function, f2::Piecewise_Function)
    return Piecewise_Function(f1 .+ f2.functions_, f2.thresholds_)
end
function +(f1::PE_Function, f2::Sum_Of_Piecewise_Functions)
    return Sum_Of_Piecewise_Functions(f2.functions_,f2.global_funcs_ + f1)
end

function +(f1::Sum_Of_Functions, f2::PE_Function)
    return +(f2,f1)
end
function +(f1::Piecewise_Function, f2::PE_Function)
    return +(f2,f1)
end
"""
    -(f1::MultivariateFunction, f2::MultivariateFunction)
    Any MultivariateFunction can be subtracted from another to form a MultivariateFunction reflecting the difference.
"""
function -(f1::PE_Function, f2::PE_Function)
    return +(f1,-1*f2)
end
function -(f1::PE_Function, f2::Sum_Of_Functions)
    return +(f1,-1*f2)
end
function -(f1::PE_Function, f2::Piecewise_Function)
    return +(f1, -1*f2 )
end
function -(f1::PE_Function, f2::Sum_Of_Piecewise_Functions)
    return +(f1,-1*f2)
end

function -(f1::Sum_Of_Functions, f2::PE_Function)
    return +(f1,1*f2)
end
function -(f1::Piecewise_Function, f2::PE_Function)
    return +(f1,-1*f2)
end
function -(f1::Sum_Of_Piecewise_Functions, f2::PE_Function)
    return +(f1,-1*f2)
end
"""
    *(f1::MultivariateFunction, f2::MultivariateFunction)
    Any two MultivariateFunctions can be multiplied to form a MultivariateFunction reflecting the product.
"""
function *(f1::PE_Function,f2::PE_Function)
    if (length(f1.units_) == 0)
        return f1.multiplier_ * f2
    elseif (length(f2.units_) == 0)
        return f2.multiplier_ * f1
    else
        f1_bases = get_bases(f1)
        f2_bases = get_bases(f2)
        min_bases = merge(min, f1_bases, f2_bases)
        f1_rebase = change_base(f1, min_bases)
        f2_rebase = change_base(f2, min_bases)
        L1 = length(f1_rebase)
        L2 = length(f2_rebase)
        if (L1 == 1) & (L2 == 1)
            f1_ = f1_rebase[1]
            f2_ = f2_rebase[1]
            new_mult = f1_.multiplier_ * f2_.multiplier_
            return PE_Function(new_mult, merge(*, f1_.units_, f2_.units_ ))
        else
            PEs = Array{PE_Function,1}()
            for f in f1_rebase
                for g in f2_rebase
                    append!(PEs, [f * g])
                end
            end
        end
        return Sum_Of_Functions(PEs)
    end
end

function *(f1::PE_Function, f2::Sum_Of_Functions)
    multiplied_functions = f1 .* f2.functions_
    return Sum_Of_Functions(multiplied_functions)
end
function *(f1::PE_Function, f2::Piecewise_Function)
    return Piecewise_Function(f1 .* f2.functions_, f2.thresholds_)
end
function *(f1::PE_Function, f2::Sum_Of_Piecewise_Functions)
    return f1 * convert(Piecewise_Function, f2)
end

function *(f1::Sum_Of_Functions, f2::PE_Function)
    return *(f2,f1)
end
function *(f1::Piecewise_Function, f2::PE_Function)
    return *(f2,f1)
end
function *(f1::Sum_Of_Piecewise_Functions, f2::PE_Function)
    return *(f2,f1)
end

# Sum of Functions

function +(f::Sum_Of_Functions,number::Float64)
    constant_function = PE_Function(number)
    return Sum_Of_Functions(vcat(f.functions_, [constant_function]))
end
function -(f::Sum_Of_Functions, number::Float64)
    return +(f, -number)
end
function *(f::Sum_Of_Functions, number::Float64)
    funcs = deepcopy(f.functions_)
    for i in 1:length(funcs)
        funcs[i] = funcs[i] * number
    end
    return Sum_Of_Functions(funcs)
end
function /(f::Sum_Of_Functions, number::Float64)
    return *(f, 1/number )
end

function +(f::Sum_Of_Functions, number::Int)
    number_as_float = convert(Float64, number)
    return +(f, number_as_float)
end
function -(f::Sum_Of_Functions, number::Int)
    number_as_float = convert(Float64, number)
    return -(f, number_as_float)
end
function *(f::Sum_Of_Functions, number::Int)
    number_as_float = convert(Float64, number)
    return *(f, number_as_float)
end
function /(f::Sum_Of_Functions, number::Int)
    number_as_float = convert(Float64, number)
    return /(f, number_as_float)
end
function ^(f::MultivariateFunction, number::Int)
    if number < 0
        error("Negative powers are not supported.")
    elseif number == 0
        return PE_Function(1.0)
    elseif number == 1
        return f
    else
        val = f * f
        for i in 3:number
            val = val * f
        end
        return val
    end
end
function ^(f::Sum_Of_Functions, number::Float64)
    error("Cannot raise a Sum_Of_Functions to a float value.")
end

function +(f1::Sum_Of_Functions, f2::Sum_Of_Functions)
    return Sum_Of_Functions([f1,f2])
end
function +(f1::Sum_Of_Functions, f2::Piecewise_Function)
    funcs = deepcopy(f2.functions_)
    for f in f1.functions_
        funcs = funcs .+ f
    end
    return Piecewise_Function(funcs, f2.thresholds_)
end
function +(f1::Sum_Of_Functions, f2::Sum_Of_Piecewise_Functions)
    return Sum_Of_Piecewise_Functions(f2.functions_,f2.global_funcs_ + f1)
end
function +(f1::Piecewise_Function, f2::Sum_Of_Functions)
    return +(f2,f1)
end
function +(f1::Sum_Of_Piecewise_Functions, f2::Sum_Of_Functions)
    return +(f2,f1)
end

function -(f1::Sum_Of_Functions, f2::Sum_Of_Functions)
    return Sum_Of_Functions([f1,-1*f2])
end
function -(f1::Sum_Of_Functions, f2::Piecewise_Function)
    return +(f1, -1 * f2)
end
function -(f1::Sum_Of_Functions, f2::Sum_Of_Piecewise_Functions)
    return +(f1,-1*f2)
end

function -(f1::Piecewise_Function, f2::Sum_Of_Functions)
    return +(f1, -1*f2)
end
function -(f1::Sum_Of_Piecewise_Functions, f2::Sum_Of_Functions)
    return +(f1,-1*f2)
end

function *(f1::Sum_Of_Functions,f2::Sum_Of_Functions)
    results = Array{Sum_Of_Functions}(undef, length(f1.functions_))
    for i in 1:length(f1.functions_)
        new_funcs = f1.functions_[i] * f2
        results[i] = new_funcs
    end
    return Sum_Of_Functions(results)
end
function *(f1::Sum_Of_Functions, f2::Piecewise_Function)
    f1_funcs = f1.functions_
    if length(f1_funcs) == 0
        return PE_Function(0.0)
    end
    funcs = f1_funcs[1] .* f2.functions_
    if length(f1_funcs) == 1
        return Piecewise_Function(funcs, f2.thresholds_)
    else
        for i in 2:length(f1_funcs)
            funcs = funcs .+ (f1_funcs[i] .* f2.functions_)
        end
        return Piecewise_Function(funcs, f2.thresholds_)
    end
end
function *(f1::Sum_Of_Functions, f2::Sum_Of_Piecewise_Functions)
    return f1 * convert(Piecewise_Function, f2)
end

function *(f1::Piecewise_Function, f2::Sum_Of_Functions)
    return *(f2,f1)
end
function *(f1::Sum_Of_Piecewise_Functions, f2::Sum_Of_Functions)
    return *(f2,f1)
end

# Piecewise Functions

function +(f::Piecewise_Function,number::Float64)
    functions_with_addition = f.functions_ .+ number
    return Piecewise_Function(functions_with_addition, f.thresholds_)
end
function -(f::Piecewise_Function,number::Float64)
    return +(f,-1.0*number)
end
function *(f::Piecewise_Function,number::Float64)
    functions_with_multiplication = f.functions_ .* number
    return Piecewise_Function(functions_with_multiplication, f.thresholds_)
end
function /(f::Piecewise_Function,number::Float64)
    return *(f, 1.0/number)
end
function +(f::Piecewise_Function,number::Int)
    number_as_float = convert(Float64, number)
    return +(f,number_as_float)
end
function -(f::Piecewise_Function,number::Int)
    number_as_float = convert(Float64, number)
    return -(f,number_as_float)
end
function *(f::Piecewise_Function,number::Int)
    number_as_float = convert(Float64, number)
    return *(f,number_as_float)
end
function /(f::Piecewise_Function,number::Int)
    number_as_float = convert(Float64, number)
    return /(f,number_as_float)
end

function +(f1::Piecewise_Function,f2::Piecewise_Function)
    c_f1, c_f2 = create_common_pieces(f1,f2)
    thresholds_ = c_f1.thresholds_
    functions_  = c_f1.functions_ + c_f2.functions_
    return Piecewise_Function(functions_, thresholds_)
end
function +(f1::Piecewise_Function,f2::Sum_Of_Piecewise_Functions)
    return Sum_Of_Piecewise_Functions(vcat(f2.functions_, f1), f2.global_funcs_)
end
function +(f1::Sum_Of_Piecewise_Functions,f2::Piecewise_Function)
    return +(f2,f1)
end

function -(f1::Piecewise_Function,f2::Piecewise_Function)
    return +(f1, -1.0 * f2)
end
function -(f1::Piecewise_Function,f2::Sum_Of_Piecewise_Functions)
    return +(f1,-1*f2)
end
function -(f1::Sum_Of_Piecewise_Functions,f2::Piecewise_Function)
    return +(f1,-1*f2)
end
function *(f1::Piecewise_Function,f2::Piecewise_Function)
    c_f1, c_f2 = create_common_pieces(f1,f2)
    thresholds_ = c_f1.thresholds_
    functions_ = c_f1.functions_ .* c_f2.functions_
    return Piecewise_Function(functions_, thresholds_)
end
function *(f1::Piecewise_Function,f2::Sum_Of_Piecewise_Functions)
    return f1 * convert(Piecewise_Function, f2)
end
function *(f1::Sum_Of_Piecewise_Functions,f2::Piecewise_Function)
    return *(f2,f1)
end

# Sum of Piecewise Functions
function +(f::Sum_Of_Piecewise_Functions,number::Float64)
    return Sum_Of_Piecewise_Functions(f.functions_, f.global_funcs_ + number )
end
function -(f::Sum_Of_Piecewise_Functions,number::Float64)
    return +(f,-1.0*number)
end
function *(f::Sum_Of_Piecewise_Functions,number::Float64)
    return Sum_Of_Piecewise_Functions(number .* f.functions_, number * f.global_funcs_)
end
function /(f::Sum_Of_Piecewise_Functions,number::Float64)
    return *(f, 1.0/number)
end
function +(f::Sum_Of_Piecewise_Functions,number::Int)
    number_as_float = convert(Float64, number)
    return +(f,number_as_float)
end
function -(f::Sum_Of_Piecewise_Functions,number::Int)
    number_as_float = convert(Float64, number)
    return -(f,number_as_float)
end
function *(f::Sum_Of_Piecewise_Functions,number::Int)
    number_as_float = convert(Float64, number)
    return *(f,number_as_float)
end
function /(f::Sum_Of_Piecewise_Functions,number::Int)
    number_as_float = convert(Float64, number)
    return /(f,number_as_float)
end

function +(f1::Sum_Of_Piecewise_Functions,f2::Sum_Of_Piecewise_Functions)
    return Sum_Of_Piecewise_Functions(vcat(f1.functions_, f2.functions_), f1.global_funcs_ + f2.global_funcs_)
end

function -(f1::Sum_Of_Piecewise_Functions,f2::Sum_Of_Piecewise_Functions)
    return +(f1,-1*f2)
end
function *(f1::Sum_Of_Piecewise_Functions,f2::Sum_Of_Piecewise_Functions)
    return convert(Piecewise_Function,f1) * convert(Piecewise_Function, f2)
end

function +(f1::Missing,f2::MultivariateFunction)
    return f1
end
function -(f1::Missing,f2::MultivariateFunction)
    return f1
end
function *(f1::Missing,f2::MultivariateFunction)
    return f1
end
function +(f1::MultivariateFunction,f2::Missing)
    return f2
end
function -(f1::MultivariateFunction,f2::Missing)
    return f2
end

function *(f1::MultivariateFunction,f2::Missing)
    return f2
end

# All

function +(number::Union{Int,Float64}, f::MultivariateFunction)
    return f + number
end
function -(number::Union{Int,Float64}, f::MultivariateFunction)
    return +(number,-1*f)
end
function *(number::Union{Int,Float64}, f::MultivariateFunction)
    return f * number
end
function /(number::Union{Int,Float64}, f::MultivariateFunction)
    error("This package doesn't support dividing by a function.")
end
function ^(number::Union{Int,Float64}, f::MultivariateFunction)
    error("This package doesn't support raising to the power of a function.")
end

###
function integral(f::MultivariateFunction, limits::Dict{Symbol,Tuple{Any,Any}})
    new_limits = convert_to_conformable_dict(limits)
    return integral(f, new_limits)
end
function evaluate(f::MultivariateFunction, coordinates::Dict{Symbol,Any})
    new_coordinates = convert_to_conformable_dict(coordinates)
    return evaluate(f, new_coordinates)
end

###
function convert_to_linearly_rescale_inputs(f::PE_Unit, alpha::Float64, beta::Float64)
    # We want the change the function so that whenever we put in x it is like we put in alpha x + beta.
    beta = beta / alpha
    alpha = 1.0/alpha
    new_base_ = (f.base_ + beta)/alpha
    new_multiplier = alpha^(f.d_)
    new_power_ = f.b_ * alpha
    return new_multiplier, PE_Unit(new_power_, new_base_, f.d_)
end
function convert_to_linearly_rescale_inputs(f::Missing, alpha_beta::Dict{Symbol,Tuple{Float64,Float64}})
    return f
end
function convert_to_linearly_rescale_inputs(f::PE_Function, alpha_beta::Dict{Symbol,Tuple{Float64,Float64}})
    if length(f.units_) == 0
        return f
    end
    mult = f.multiplier_
    final_units = Dict{Symbol,PE_Unit}()
    for dd in setdiff(keys(f.units_), keys(alpha_beta))
        final_units[dd] = f.units_[dd]
    end
    for dim in keys(alpha_beta)
        mm, unit = convert_to_linearly_rescale_inputs(f.units_[dim], alpha_beta[dim][1], alpha_beta[dim][2])
        final_units[dim] = unit
        mult = mult * mm
    end
    return PE_Function(mult, final_units)
end
function convert_to_linearly_rescale_inputs(f::Sum_Of_Functions, alpha_beta::Dict{Symbol,Tuple{Float64,Float64}})
    funcs = convert_to_linearly_rescale_inputs.(f.functions_, Ref(alpha_beta))
    return Sum_Of_Functions(funcs)
end
function convert_to_linearly_rescale_inputs(f::Piecewise_Function, alpha_beta::Dict{Symbol,Tuple{Float64,Float64}})
    new_thresholds = OrderedDict{Symbol,Array{Float64,1}}()
    for k in keys(f.thresholds_)
        alpha = alpha_beta[k][1]
        beta  = alpha_beta[k][2]
        new_thresholds[k] = (f.thresholds_[k] .* alpha) .+ beta
    end
    funcs_ = convert_to_linearly_rescale_inputs.(f.functions_, Ref(alpha_beta))
    return Piecewise_Function(funcs_, new_thresholds)
end

function convert_to_linearly_rescale_inputs(f::MultivariateFunction, alpha::Float64, beta::Float64)
    alpha_beta= Dict{Symbol,Tuple{Float64,Float64}}(default_symbol => (alpha,beta))
    return convert_to_linearly_rescale_inputs(f, alpha_beta)
end


using SwitchTimeOpt
using MathOptInterface
const MOI = MathOptInterface

using Plots
using Ipopt

# Define Solver options
maxiter = 100
maxtime = 1000.0
verbose = 5
tolerance = 1e-06

#Define solver
solver = Ipopt.Optimizer()
MOI.set(solver, MOI.RawOptimizerAttribute("tol"), tolerance)
MOI.set(solver, MOI.RawOptimizerAttribute("print_level"), verbose)
MOI.set(solver, MOI.RawOptimizerAttribute("max_cpu_time"), maxtime)
MOI.set(solver, MOI.RawOptimizerAttribute("max_iter"), maxiter)

### Define system parameters
# Size of the state space
nx = 2;

# Number of switches
N = 5;

# Initial state
x0 = [1.0; 1.0]

# Define initial and final time
t0 = 0.0
tf = 1.0

### Define system dynamics
A = zeros(nx, nx, N+1)

A[:, :, 1] = [-1 0;
              1  2]
A[:, :, 2] = [1 1;
              1 -2]
for i = 3:N+1
  A[:, :, i] = A[:, :, mod(i+1, 2)+1]
end

### Define and solve switching time optimization problem
# m = stoproblem(x0, A, solver=solver)
#
m = stoproblem(x0, A, t0=t0, tf=tf, ngrid=100)

# Solve problem
solve!(m)

# Obtain values
tauopt = gettau(m)
Jopt = getobjval(m)
soltime = getsoltime(m)

# Simulate linear system
xsim, xpts, Jsim, t = simulate(m)
println("Objective: ", Jsim)

uvec = zeros(2,N+1)
for i=1:N+1
  uvec[mod(i+1,2)+1,i] = 1
end

plotSolution(m, uvec, "Linear example", "Linear example", true, false)


# Unit test functions
function unit_test_rmheap_empty()
    r = RMHeap{Int}()
    @test r.N == 0

    return nothing
end

function unit_test_rmheap_empty_isheap()
    r = RMHeap{Int}()
    @test isheap(r)

    return nothing
end

function unit_test_rmheap_add()
    r = RMHeap{Int}()
    val1 = 1
    val2 = 2
    val3 = 3
    val4 = 4
    val5 = 5
    add!(r, val1)
    add!(r, val2)
    @test r.root.key == val1
    @test r.root.right.key == val2

    return nothing
end

function unit_test_rmheap_remove()
    n = 10^4
    r = RMHeap{Int}()

    @time for i = 1:n
        add!(r, i)
    end

    remove!(r)

    return true
end

function unit_test_minpq_size()
    r = RMHeap{Int}()
    @test size(r) == 0
    val1 = 5
    val2 = 4
    add!(r, val1)
    @test size(r) == 1
    add!(r, val2)
    @test size(r) == 2

    remove!(r)
    @test size(r) == 1

    remove!(r)
    @test size(r) == 0

    return nothing
end

function unit_test_minpq_getmin()
    r = RMHeap{Int}()
    val1 = 5
    val2 = 4
    val3 = 3
    val4 = 2
    val5 = 1
    add!(r, val1)
    add!(r, val2)
    add!(r, val3)
    add!(r, val4)
    add!(r, val5)

    @test getmin(r) == 1

    return nothing
end

function unit_test_minpq_isempty()
    r = RMHeap{Int}()
    @test isempty(r)
    val1 = 5
    add!(r, val1)
    @test isempty(r) == false

    return nothing
end

function unit_test_rmheap_example()
    n = 10^3
    r = RMHeap{Int}()

    for i in shuffle(1:n)
        add!(r, i)
        @test isheap(r)
    end

    for i in 1:n
        @test getmin(r) == i
        @test remove!(r) == i
        @test isheap(r)
    end
    @test isempty(r)

    return nothing
end

function unit_test_rmheap_empty_isheap()
    r = RMHeap{Int}()
    @test isheap(r)

    return nothing
end

function unit_test_minpq_empty()
    r = MinPQ{Int}()
    @test r.N == 0

    return nothing
end

function unit_test_minpq_empty_isheap()
    r = MinPQ{Int}()
    @test isheap(r)

    return nothing
end

function unit_test_minpq_add()
    r = MinPQ{Int}()
    val1 = 1
    val2 = 2
    val3 = 3
    val4 = 4
    val5 = 5
    add!(r, val5)
    @test isheap(r)
    add!(r, val4)
    @test isheap(r)
    add!(r, val3)
    @test isheap(r)
    add!(r, val2)
    @test isheap(r)
    add!(r, val1)
    @test isheap(r)
    @test r.root[1] == val1
    @test r.root[2] == val2
    @test r.root[3] == val4
    @test r.root[4] == val5
    @test r.root[5] == val3

    return nothing
end

function unit_test_minpq_greater()
    r = MinPQ{Int}()
    val1 = 1
    val2 = 2
    val3 = 3
    val4 = 4
    val5 = 5
    add!(r, val5)
    add!(r, val4)
    add!(r, val3)
    add!(r, val2)
    add!(r, val1)
    @test greater(r, 2, 1)
    @test greater(r, 1, 2) == false

    return nothing
end

function unit_test_minpq_remove()
    r = MinPQ{Int}()
    val1 = 1
    val2 = 2
    val3 = 3
    val4 = 4
    val5 = 5
    add!(r, val5)
    add!(r, val4)
    add!(r, val3)
    add!(r, val2)
    add!(r, val1)

    @test remove!(r) == val1
    @test isheap(r)
    @test r.root[1] == val2
    @test r.root[2] == val3
    @test r.root[3] == val4
    @test r.root[4] == val5

    @test remove!(r) == val2
    @test isheap(r)
    @test r.root[1] == val3
    @test r.root[2] == val5
    @test r.root[3] == val4

    @test remove!(r) == val3
    @test isheap(r)
    @test r.root[1] == val4
    @test r.root[2] == val5

    @test remove!(r) == val4
    @test isheap(r)
    @test r.root[1] == val5

    @test remove!(r) == val5
    @test isheap(r)

    @test remove!(r) === nothing
    @test isempty(r)
    @test isheap(r)

    return nothing
end

function unit_test_minpq_size()
    r = MinPQ{Int}()
    @test size(r) == 0
    val1 = 5
    val2 = 4
    add!(r, val1)
    @test size(r) == 1
    add!(r, val2)
    @test size(r) == 2

    remove!(r)
    @test size(r) == 1

    remove!(r)
    @test size(r) == 0

    return nothing
end

function unit_test_minpq_getmin()
    r = MinPQ{Int}()
    val1 = 5
    val2 = 4
    val3 = 3
    val4 = 2
    val5 = 1
    add!(r, val1)
    add!(r, val2)
    add!(r, val3)
    add!(r, val4)
    add!(r, val5)

    @test getmin(r) == 1

    return nothing
end

function unit_test_minpq_isempty()
    r = MinPQ{Int}()
    @test isempty(r)
    val1 = 5
    add!(r, val1)
    @test isempty(r) == false

    return nothing
end

function unit_test_minpq_example()
    n = 10^3
    r = MinPQ{Int}()

    for i in shuffle(1:n)
        add!(r, i)
        @test isheap(r)
    end

    for i in 1:n
        @test getmin(r) == i
        @test remove!(r) == i
        @test isheap(r)
    end
    @test isempty(r)

    return nothing
end

# Run the unit tests
@testset "Randomised meldable heaps" begin
    unit_test_rmheap_empty()
    unit_test_rmheap_empty_isheap()
    #unit_test_minpq_add() # bit harder, random
    #unit_test_minpq_remove() # bit harder, random
end

@testset "Minimum priority queue   " begin
    #unit_test_minpq_sink()
    #unit_test_minpq_swim()
end