math.hpp

Go to the documentation of this file.

#pragma once
 
#include <Eigen/Core>
#include <Eigen/Eigenvalues> 
 
#include <cstdint>
 
namespace controlpp{
 
    template<class T>
    constexpr T product_over(T from, T to){
        unsigned long product = 1;
        for(; from < to; ++from){
            product *= from;
        }
        return product;
    }
 
    template<class T>
    constexpr T pow(const T& base, const int& exp){
        T result = static_cast<T>(1);
        if(exp >= 0){
            for(int i = 0; i < exp; ++i){
                result *= base;
            }
        }else{
            for(int i = 0; i < -exp; ++i){
                result /= base;
            }
        }
        return result;
    }
 
    template<class T, int N>
    Eigen::Vector<T, N> unwrap(const Eigen::Vector<T, N>& y, const T& modulo){
        Eigen::Vector<T, N> result;
        if constexpr (N == Eigen::Dynamic) result.resize(y.size());
        T offset = 0;
        result(0) = y(0);
        for(int i = 1; i < y.size(); ++i){
            while(((y(i) + offset) - result(i-1)) > (modulo/2)){
                offset -= modulo;
            }
            while(((y(i) + offset) - result(i-1)) < (-modulo/2)){
                offset += modulo;
            }
            result(i) = y(i) + offset;
        }
        return result;
    }
 
    template<class T, int N>
    Eigen::Vector<T, N> unwrap_rad(const Eigen::Vector<T, N>& phases){
        const T modulo = std::numbers::pi_v<T> * static_cast<T>(2);
        return unwrap(phases, modulo);
    }
 
    template<class T, int N>
    Eigen::Vector<T, N> unwrap_deg(const Eigen::Vector<T, N>& phases){
        const T modulo = 360;
        return unwrap(phases, modulo);
    }
 
    
    template<class T = double>
    constexpr T pade_param_reccursice(T P_k, std::int32_t m, std::int32_t n, std::int32_t k){
        const std::int64_t num = (m - k);
        const std::int64_t den = (k + 1) * (m + n - k);
        const double f = static_cast<T>(num) / static_cast<T>(den);
        return P_k * f;
    }
 
    template<class T = double>
    constexpr void pade_params(T* params, std::size_t size, std::uint32_t m, std::uint32_t n){
        params[0] = T(1.0);
        for(std::size_t k = 1; k < size; ++k){
            params[k] = pade_param_reccursice(params[k-1], m, n, k-1);
        }
    }
 
    template<class T = double>
    constexpr T pade_num_param(std::int32_t m, std::int32_t n, std::int32_t k){
        double P_k = 1.0;
        for(std::int32_t i = 0; i < k; ++i){
            P_k = pade_param_reccursice(P_k, m, n, i);
        }
        return P_k;
    }
 
    template<class T = double>
    constexpr T pade_den_param(unsigned long m, unsigned long n, unsigned long k){
        return pade_num_param(n, m, k);
    }
 
    template<class T, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
    Eigen::Matrix<T, Rows, Cols, Options, MaxRows, MaxCols> identity_like([[maybe_unused]]const Eigen::Matrix<T, Rows, Cols, Options, MaxRows, MaxCols>& unused){
        if constexpr (Rows != Eigen::Dynamic && Cols != Eigen::Dynamic){
            return Eigen::Matrix<T, Rows, Cols, Options, MaxRows, MaxCols>::Identity();
        }else{
            Eigen::Matrix<T, Rows, Cols, Options, MaxRows, MaxCols> result;
            result.setIdentity();
            return result;
        }
    }
 
    template<class T, int LSize, int RSize>
    Eigen::Matrix<T, LSize + RSize, LSize + RSize> join_to_diagonal(const Eigen::Vector<T, LSize>& l, const Eigen::Vector<T, RSize>& r){
        Eigen::Matrix<T, LSize + RSize, LSize + RSize> result = Eigen::Matrix<T, LSize + RSize, LSize + RSize>::Zero();
        result.diagonal().head(LSize) = l;
        result.diagonal().tail(RSize) = r;
        return result;
    }
 
    template<class T, int LSize, int RSize>
    requires(LSize != Eigen::Dynamic && RSize != Eigen::Dynamic)
    Eigen::Vector<T, LSize + RSize> join_to_vector(const Eigen::Vector<T, LSize>& l, const Eigen::Vector<T, RSize>& r){
        Eigen::Vector<T, LSize + RSize> result;
        result.head(LSize) = l;
        result.tail(RSize) = r;
        return result;
    }
 
    template<class T, int LSize, int RSize>
    requires(!(LSize != Eigen::Dynamic && RSize != Eigen::Dynamic))
    Eigen::Vector<T, Eigen::Dynamic> join_to_vector(const Eigen::Vector<T, LSize>& l, const Eigen::Vector<T, RSize>& r){
        Eigen::Vector<T, Eigen::Dynamic> result(l.size() + r.size());
        result.head(l.size()) = l;
        result.tail(r.size()) = r;
        return result;
    }
 
    template<class T, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
    Eigen::Matrix<T, Rows-1, Cols-1> minor(const Eigen::Matrix<T, Rows, Cols, Options, MaxRows, MaxCols>& A, size_t ex_row, size_t ex_col){
        Eigen::Matrix<T, Rows-1, Cols-1> result;
        size_t A_row = 0;
        size_t A_col = 0;
        for(size_t result_row = 0; result_row < (Rows-1); ++result_row, (void)++A_row){
            for(size_t result_col = 0; result_col < (Cols-1); ++result_col, (void)++A_col){
                A_row += (A_row == ex_row) ? 1 : 0;
                A_col += (A_col == ex_col) ? 1 : 0;
                result(result_row, result_col) = A(A_row, A_col);
            }
        }
        return result;
    }
 
    template<class T, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
    Eigen::Matrix<T, Rows, Cols> adj(const Eigen::Matrix<T, Rows, Cols, Options, MaxRows, MaxCols>& A){
        Eigen::Matrix<T, Rows, Cols> result;
        for(int row = 0; row < Rows; ++row){
            const bool row_sign = ((row & 1) == 1); // even are positive, odd are negative
            for(int col = 0; col < Cols; ++col){
                const bool col_sign = ((col & 1) == 1); // even are positive, odd are negative
                const bool value_sign = row_sign != col_sign; // sign of the value (both positive/negative: value is positive) (different signs: value is negative)
                const auto min = minor(A, row, col);
                const T det = min.determinant();
                if(value_sign){
                    // negative sign
                    result(col, row) = -det;
                }else{
                    // positive sign
                    result(col, row) = det;
                }
            }
        }
        return result;
    }
 
    template<class T, int N>
    Eigen::Matrix<T, N-1, N-1> companion(const Eigen::Vector<T, N>& v){
        Eigen::Matrix<T, N-1, N-1> result;
        result.template block<N-2, N-2>(0, 1) = Eigen::Matrix<T, N-2, N-2>::Identity();
        result.col(0).setZero();
        result.row(N-2) = -v.head(N-1) / v(N-1);
        return result;
    }
 
    template<class T, int N>
    requires(N % 2 == 0)
    Eigen::Matrix<T, N/2, N/2> hamilton_solver(
        const Eigen::Matrix<T, N, N> H
    ){
        // 2. Compute stable eigenvectors
        Eigen::ComplexEigenSolver<Eigen::Matrix<T, N, N>> ces;
        ces.compute(H);
        const auto& H_eigvals = ces.eigenvalues();
        const auto& H_eigvecs = ces.eigenvectors();
 
        // in a 2n hamilton matrix are exactly n stable ones
        Eigen::Matrix<std::complex<T>, N, N/2> StableEigenVecs;
        int si = 0; // stable eigenvalue iterator
        int ei = 0; // eigen value iterator
        for(; (si < N/2) && (ei < N); ++ei){
            if(H_eigvals(ei).real() <= 0){// only stable ones
                StableEigenVecs.col(si) = H_eigvecs.col(ei);
                ++si;
            }
        }
 
        // 3. Repartition
        const auto TopPartition = StableEigenVecs.template block<N/2, N/2>(0, 0);
        const auto BottomPartition = StableEigenVecs.template block<N/2, N/2>(N/2, 0);
 
        // 4. Recover Solution (BottomPartition * TopPartition^-1)
        const Eigen::Matrix<std::complex<T>, N/2, N/2> X = TopPartition.transpose().partialPivLu().solve(BottomPartition.transpose()).transpose();
 
        // The result is real, make sure it is real because it should be
        const Eigen::Matrix<T, N/2, N/2> realX = X.real();
 
        // force the result X to be symetric to combat small numerical errors
        const Eigen::Matrix<T, N/2, N/2> result = static_cast<T>(0.5) * (realX + realX.transpose());
        return result;
    }
 
    template<class T, int N>
    Eigen::Matrix<T, N/2, N/2> symplectic_solver(
        const Eigen::Matrix<T, N, N> S
    ){
        // 2. Compute stable eigenvectors
        Eigen::ComplexEigenSolver<Eigen::Matrix<T, N, N>> ces;
        ces.compute(S);
        const auto& eigvals = ces.eigenvalues();
        const auto& eigvecs = ces.eigenvectors();
 
        // in a 2n hamilton matrix are exactly n stable ones
        Eigen::Matrix<std::complex<T>, N, N/2> StableEigenVecs;
        StableEigenVecs.setZero();
        int si = 0; // stable eigenvalue iterator
        int ei = 0; // eigen value iterator
        for(; (si < N/2) && (ei < N); ++ei){
            if(std::abs(eigvals(ei)) < 1){// only stable ones
                StableEigenVecs.col(si) = eigvecs.col(ei);
                ++si;
            }
        }
 
        // 3. Repartition
        const auto TopPartition = StableEigenVecs.template block<N/2, N/2>(0, 0);
        const auto BottomPartition = StableEigenVecs.template block<N/2, N/2>(N/2, 0);
 
        // 4. Recover Solution (BottomPartition * TopPartition^-1)
        const Eigen::Matrix<std::complex<T>, N/2, N/2> X = TopPartition.transpose().partialPivLu().solve(BottomPartition.transpose()).transpose();
 
        // The result is real, make sure it is real because it should be
        const Eigen::Matrix<T, N/2, N/2> realX = X.real();
 
        // force the result X to be symetric to combat small numerical errors
        const Eigen::Matrix<T, N/2, N/2> result = static_cast<T>(0.5) * (realX + realX.transpose());
 
        return result;
    }
 
    template<class T, int NStates,
            int AOpt, int AMaxR, int AMaxC,
            int QOpt, int QMaxR, int QMaxC
    >
    Eigen::Matrix<T, NStates, NStates> lyapunov_solver(
        const Eigen::Matrix<T, NStates, NStates, AOpt, AMaxR, AMaxC>& A,
        const Eigen::Matrix<T, NStates, NStates, QOpt, QMaxR, QMaxC>& Q
    ){
        // 1. Build the hamilton matrix
        Eigen::Matrix<T, 2*NStates, 2*NStates> H;
        H.topLeftCorner(NStates, NStates) = A;
        H.topRightCorner(NStates, NStates).setZero();
        H.bottomLeftCorner(NStates, NStates) = - Q;
        H.bottomRightCorner(NStates, NStates) = -A.transpose();
        return hamilton_solver(H);
    }
 
    template<class ValueType, int NStates, int NInputs, int NOutputs>
    std::tuple<Eigen::Matrix<ValueType, NStates, NStates>, Eigen::Matrix<ValueType, NStates, NStates>> energy_scaling_matrix(
        const Eigen::Matrix<ValueType, NStates, NStates> A,
        const Eigen::Matrix<ValueType, NStates, NInputs> B,
        const Eigen::Matrix<ValueType, NOutputs, NStates> C
    ){
        // Calculate the two gramians Wc and Wo
        const Eigen::Matrix<ValueType, NStates, NStates> Bsqr = B * B.transpose();
        const Eigen::Matrix<ValueType, NStates, NStates> Wc = lyapunov_solver(A.transpose(), Bsqr);
 
        const Eigen::Matrix<ValueType, NStates, NStates> Csqr = C.transpose() * C;
        const Eigen::Matrix<ValueType, NStates, NStates> Wo = lyapunov_solver(A, Csqr);
 
        // force symetry and add some to the diagonal for numerical stability
        const ValueType eps = std::numeric_limits<ValueType>::epsilon();
 
        Eigen::Matrix<ValueType, NStates, NStates> Wc_sym = (Wc + Wc.transpose()) * static_cast<ValueType>(0.5);
        {
            ValueType jitter = std::max(ValueType(1), Wc_sym.diagonal().cwiseAbs().maxCoeff());
            Wc_sym.diagonal().array() += jitter * eps;
        }
        Eigen::Matrix<ValueType, NStates, NStates> Wo_sym = (Wo + Wo.transpose()) * static_cast<ValueType>(0.5);
        {
            ValueType jitter = std::max(ValueType(1), Wo_sym.diagonal().cwiseAbs().maxCoeff());
            Wo_sym.diagonal().array() += jitter * eps;
        }
 
        // calculate the cholesky factors using LLT
        const Eigen::LLT<Eigen::Matrix<ValueType, NStates, NStates>> Wc_colesky(Wc_sym);
        const Eigen::LLT<Eigen::Matrix<ValueType, NStates, NStates>> Wo_colesky(Wo_sym);
 
        // partition into triangle forms
        const Eigen::Matrix<ValueType, NStates, NStates> Rc = Wc_colesky.matrixU();
        const Eigen::Matrix<ValueType, NStates, NStates> Ro = Wo_colesky.matrixL();
 
        // calculate the SVD
        const Eigen::Matrix<ValueType, NStates, NStates> M = Ro * Rc;
        const Eigen::JacobiSVD<Eigen::Matrix<ValueType, NStates, NStates>> svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV);
        const Eigen::Matrix<ValueType, NStates, NStates> U  = svd.matrixU();
        const Eigen::Matrix<ValueType, NStates, 1> s  = svd.singularValues();
        const Eigen::Matrix<ValueType, NStates, NStates> V = svd.matrixV();
 
        
        const Eigen::Matrix<ValueType, NStates, 1> s_sqrt = (s.array().max(eps)).sqrt().matrix();
        const Eigen::Matrix<ValueType, NStates, NStates> S_sqrt = s_sqrt.asDiagonal();      
 
        // Finally!!! we can calculate the resulting transformation matrices
        const Eigen::Matrix<ValueType, NStates, NStates> T = Rc.transpose().template triangularView<Eigen::Lower>().solve((S_sqrt * V.transpose()).transpose()).transpose();
        const Eigen::Matrix<ValueType, NStates, NStates> T_inverse = Ro.template triangularView<Eigen::Lower>().solve(U * S_sqrt);
 
        return {T, T_inverse};
    }
 
    
 
    template<class T, int NStates, int NInputs,
            int AOpt, int AMaxR, int AMaxC,
            int BOpt, int BMaxR, int BMaxC,
            int ROpt, int RMaxR, int RMaxC,
            int QOpt, int QMaxR, int QMaxC
    >
    Eigen::Matrix<T, NStates, NStates> care_solver(
        const Eigen::Matrix<T, NStates, NStates, AOpt, AMaxR, AMaxC>& A,
        const Eigen::Matrix<T, NStates, NInputs, BOpt, BMaxR, BMaxC>& B,
        const Eigen::Matrix<T, NStates, NStates, QOpt, QMaxR, QMaxC>& Q,
        const Eigen::Matrix<T, NInputs, NInputs, ROpt, RMaxR, RMaxC>& R
    ){
        Eigen::Matrix<T, 2*NStates, 2*NStates> H;
        H.topLeftCorner(NStates, NStates) = A;
        H.topRightCorner(NStates, NStates) =  -B * R.ldlt().solve(B.transpose());
        H.bottomLeftCorner(NStates, NStates) = -Q;
        H.bottomRightCorner(NStates, NStates) = -A;
        return hamilton_solver(H);
    }
 
    template<class T, int NStates, int NInputs,
            int AOpt, int AMaxR, int AMaxC,
            int BOpt, int BMaxR, int BMaxC,
            int ROpt, int RMaxR, int RMaxC,
            int QOpt, int QMaxR, int QMaxC
    >
    Eigen::Matrix<T, NStates, NStates> dare_eigenvector_solver(
        const Eigen::Matrix<T, NStates, NStates, AOpt, AMaxR, AMaxC>& A,
        const Eigen::Matrix<T, NStates, NInputs, BOpt, BMaxR, BMaxC>& B,
        const Eigen::Matrix<T, NStates, NStates, QOpt, QMaxR, QMaxC>& Q,
        const Eigen::Matrix<T, NInputs, NInputs, ROpt, RMaxR, RMaxC>& R
    ){
        const Eigen::Matrix<T, NStates, NStates> brb = B * R.llt().solve(B.transpose());
        Eigen::ColPivHouseholderQR<Eigen::Matrix<T, NStates, NStates, AOpt, AMaxR, AMaxC>> At_qr(A.transpose());
        const Eigen::Matrix<T, NStates, NStates> aq = At_qr.solve(Q);
        Eigen::Matrix<T, 2*NStates, 2*NStates> S;
        S.topLeftCorner(NStates, NStates) = A + brb * aq;
        S.topRightCorner(NStates, NStates) = -A.colPivHouseholderQr().solve(brb.transpose()).transpose();
        S.bottomLeftCorner(NStates, NStates) = - aq;
        S.bottomRightCorner(NStates, NStates) = At_qr.solve(identity_like(A));
        return symplectic_solver(S);
    }
 
    template<class T, int NStates, int NInputs,
            int AOpt, int AMaxR, int AMaxC,
            int BOpt, int BMaxR, int BMaxC,
            int ROpt, int RMaxR, int RMaxC,
            int QOpt, int QMaxR, int QMaxC
    >
    Eigen::Matrix<T, 2*NStates + NInputs, 2*NStates + NInputs> build_dare_symplectic_pencil_H(
        const Eigen::Matrix<T, NStates, NStates, AOpt, AMaxR, AMaxC>& A,
        const Eigen::Matrix<T, NStates, NInputs, BOpt, BMaxR, BMaxC>& B,
        const Eigen::Matrix<T, NStates, NStates, QOpt, QMaxR, QMaxC>& Q,
        const Eigen::Matrix<T, NInputs, NInputs, ROpt, RMaxR, RMaxC>& R
    ){
        constexpr int H_rows = 2*NStates + NInputs;
        constexpr int H_cols = 2*NStates + NInputs;
 
        // build the matrix:
        // [  A   0   B ]
        // [ -Q   I   0 ]
        // [  0   0   R ]
        Eigen::Matrix<T, H_rows, H_cols> H;
        H.setZero();
        H.template block<NStates, NStates>(0, 0) = A;
        H.template block<NStates, NInputs>(0, 2 * NStates) = B;
        H.template block<NStates, NStates>(NStates, 0) = -Q;
        H.template block<NStates, NStates>(NStates, NStates).setIdentity();
        H.template block<NInputs, NInputs>(2 * NStates, 2 * NStates) = R;
 
        return H;
    }
 
    template<class T, int NStates, int NInputs,
            int AOpt, int AMaxR, int AMaxC,
            int BOpt, int BMaxR, int BMaxC
    >
    Eigen::Matrix<T, 2*NStates + NInputs, 2*NStates + NInputs> build_dare_symplectic_pencil_J(
        const Eigen::Matrix<T, NStates, NStates, AOpt, AMaxR, AMaxC>& A,
        const Eigen::Matrix<T, NStates, NInputs, BOpt, BMaxR, BMaxC>& B
    ){  
        constexpr int J_rows = NStates + NStates + NInputs;
        constexpr int J_cols = NStates + NStates + NInputs;
        
        // build the matrix:
        // [ I   0    B]
        // [ 0   A^T  0]
        // [ 0  -B^T  0]
        Eigen::Matrix<T, J_rows, J_cols> J;
        J.setZero();
        J.template block<NStates, NStates>(0, 0).setIdentity();
        J.template block<NStates, NInputs>(0, 2 * NStates) = B;
        J.template block<NStates, NStates>(NStates, NStates) = A.transpose();
        J.template block<NInputs, NStates>(2 * NStates, NStates) = -B.transpose();
 
        return J;
    }
 
    /* DEVELOPEMENT
    template<class T, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
    int pencil_block_size(const Eigen::Matrix<T, Rows, Cols, Options, MaxRows, MaxCols>& S, int block_start){
        assert((0 <= block_start) && (block_start < S.rows() - 1) && "block_start is out of bounds for the matrix S.");
 
        int block_size = 1;
            
        // determine the blocksize (1x1 or 2x2)
        const T sub_diag = S(block_start + 1, block_start);
        
        // zero threshold for numerical stability
        const T local_diagonal = std::abs(S(block_start, block_start)) + std::abs(S(block_start + 1, block_start + 1));
        const T epsilon  = std::numeric_limits<T>::epsilon();
        const T zero_threshold = epsilon  * std::max(local_diagonal, T(1)) * T(1024);
 
        if(block_start + 1 < PencilSize){
            if(std::abs(sub_diag) > zero_threshold){
                block_size = 2;
            }else{
                block_size = 1;
            }
        }else{
            block_size = 1;
        }
 
        return block_size;
    }
    */
 
    /* DEVELOPEMENT
    template<class DerivedS, class DerivedT>
    bool is_stable_pencil_block(
        const Eigen::MatrixBase<DerivedS>& Sb,
        const Eigen::MatrixBase<DerivedT>& Tb
    ){
        assert(Sb.rows() == Sb.cols());
        assert(Tb.rows() == Tb.cols());
        assert(Sb.rows() == Tb.rows());
        assert(Sb.rows() >= 1 && Sb.rows() <= 2);
 
        if(Sb.rows() == 1){
            // blocksize is 1
 
            // check if the eigenvalue is stable (inside the unit circle)
            const auto s = Sb(0, 0);
            const auto t = Tb(0, 0);
 
            const auto scale = std::max({
                std::abs(t),
                std::abs(s),
                Scalar(1)});
 
            const auto tolerance = std::numeric_limits<Scalar>::epsilon() * scale * Scalar(128);
 
            // avoid checks against zero for numerical stability
            if((std::abs(s) <= tolerance) && (std::abs(t) <= tolerance)){
                // both s and t are zero --> indeterminante
                throw std::runtime_error("Controlpp: Indeterminate pencil block: both S and T are zero.");
            }
 
            // avoid division by zero
            const bool stable = (std::abs(s) < std::abs(t));
            return stable;
        }else{
            // blocksize is 2
 
            // solve det(S - lambda T) = 0 for the 2x2 block to find the eigenvalues
            // build the quadratic polynomial coefficients
            //
            // 
            // det | (S00 - lambda T00)   (S01 - lambda T01) |
            //     | (S10)                (S11 - lambda T11) |
            auto a = Tb(0, 0) * Tb(1, 1);
            auto b = Sb(1, 0) * Tb(0, 1) - Sb(0, 0) * Tb(1, 1) - Sb(1, 1) * Tb(0, 0);
            auto c = Sb(0, 0) * Sb(1, 1) - Sb(0, 1) * Sb(1, 0);
 
            { // assert that the block is determinate
                const auto scale = std::max({
                    std::abs(a),
                    std::abs(b),
                    std::abs(c)
                });
 
                if(scale == Scalar(0)){
                    // all coefficients are zero --> indeterminante
                    throw std::runtime_error("Controlpp: Indeterminate pencil block: All coefficients are zero.");
                }
 
                // normalize the coefficients to avoid numerical issues
                a /= scale;
                b /= scale;
                c /= scale;
 
                const auto tolerance = std::numeric_limits<Scalar>::epsilon() * Scalar(128);
                
                // avoid checks against zero for numerical stability
                if(std::abs(a) <= tolerance){
                    if(std::abs(b) <= tolerance){
                        if(std::abs(c) <= tolerance){
                            // all coefficients are zero --> indeterminante
                            throw std::runtime_error("Controlpp: Indeterminate pencil block: Zero eigenvalues.");
                        }
                        // constant equation c = 0 --> no solution --> infinite eigenvalues --> unstable
                        return false;
                    }
                    // linear equation b lambda + c = 0 --> lambda = -c/b
                    // one finite eigenvalue and one infinite eigenvalue --> unstable
                    return false;
                }
            }
 
            // solve the quadratic equation a lambda^2 + b lambda + c = 0
            // lambda_1,2 = (-b +- sqrt(b^2 - 4ac)) / 2a
            // | lambda_1,2 | < 1 ... for stability
            //   --> | (-b +- sqrt(b^2 - 4ac)) | < | 2a |  ... for stability
            const auto discriminant = b * b - 4 * a * c;
            if(discriminant >= 0){
                // real eigenvalues
                const auto lhs_1 = std::abs(-b + std::sqrt(discriminant));
                const auto lhs_2 = std::abs(-b - std::sqrt(discriminant));
                const auto rhs = std::abs(2 * a);
 
                { // assert that the block is determinate
                    // avoid checks against zero for numerical stability 
                    const auto scale = std::max({
                    std::abs(lhs_1),
                    std::abs(lhs_2),
                    std::abs(rhs),
                    Scalar(1)});
    
                    const auto tolerance = std::numeric_limits<Scalar>::epsilon() * scale * Scalar(128);
    
                    if(std::abs(lhs_1) == DerivedS::Scalar(0) && std::abs(rhs) == DerivedT::Scalar(0)){
                        // both lhs and rhs are zero --> indeterminante
                        throw std::runtime_error("Controlpp: Indeterminate pencil block: Eigenvalues are zero.");
                    }
 
                    if(std::abs(lhs_2) == DerivedS::Scalar(0) && std::abs(rhs) == DerivedT::Scalar(0)){
                        // both lhs and rhs are zero --> indeterminante
                        throw std::runtime_error("Controlpp: Indeterminate pencil block: Eigenvalues are zero.");
                    }
                }
 
                const bool stable_1 = (lhs_1 < rhs);
                const bool stable_2 = (lhs_2 < rhs);
                const bool stable = stable_1 && stable_2;
                return stable;
            }else{
                // complex eigenvalues
                const auto lhs = std::sqrt(b * b - discriminant);
                const auto rhs = std::abs(2 * a);
                { // assert that the block is determinate
                    // avoid checks against zero for numerical stability 
                    const auto scale = std::max({
                    std::abs(lhs),
                    std::abs(rhs),
                    Scalar(1)});
    
                    const auto tolerance = std::numeric_limits<Scalar>::epsilon() * scale * Scalar(128);
    
                    if(std::abs(lhs) == DerivedS::Scalar(0) && std::abs(rhs) == DerivedT::Scalar(0)){
                        // both lhs and rhs are zero --> indeterminante
                        throw std::runtime_error("Controlpp: Indeterminate pencil block: Eigenvalues are zero.");
                    }
                }
 
                const bool stable = (lhs < rhs);
                return stable;
            }
        }
    }
    */
 
    /* DEVELOPEMENT
    template<class T, int NStates, int NInputs,
            int AOpt, int AMaxR, int AMaxC,
            int BOpt, int BMaxR, int BMaxC,
            int ROpt, int RMaxR, int RMaxC,
            int QOpt, int QMaxR, int QMaxC
    >
    Eigen::Matrix<T, NStates, NStates> dare_qz_solver(
        const Eigen::Matrix<T, NStates, NStates, AOpt, AMaxR, AMaxC>& A,
        const Eigen::Matrix<T, NStates, NInputs, BOpt, BMaxR, BMaxC>& B,
        const Eigen::Matrix<T, NStates, NStates, QOpt, QMaxR, QMaxC>& Q,
        const Eigen::Matrix<T, NInputs, NInputs, ROpt, RMaxR, RMaxC>& R
    ){
        // 1. Build the symplectic pencil matrices H and J
        constexpr int PencilSize = 2 * NStates + NInputs;
        using Pencil = Eigen::Matrix<T, PencilSize, PencilSize>;
 
        const Pencil H = build_dare_symplectic_pencil_H(A, B, Q, R);
 
        const Pencil J = build_dare_symplectic_pencil_J(A, B);
 
        // 2. Solve the generalized eigenvalue problem H v = lambda J v
        Eigen::RealQZ<Pencil> qz;
        qz.compute(H, J);
 
        if(qz.info() != Eigne::Success){
            // QZ failed to converge
            throw std::runtime_error("Controlpp: dare_qz_solver(): QZ failed to converge.");
        }
 
        Pencil S = qz.matrixS(); // is quasi-upper-triangular
        Pencil Tmat = qz.matrixT(); // is upper-triangular
        Pencil Qz = qz.matrixQ(); // contains the left generalized Schur vectors
        Pencil Z = qz.matrixZ(); // contains the right generalized Schur vectors
 
        // 3. Reorder
        int block_start = 0;
        while(block_start < PencilSize){
            
            const int block_size = pencil_block_size(S, block_start);
 
            // map to eigen::map for easier access
            auto Sb = S.block(block_start, block_start, block_size, block_size);
            auto Tb = Tmat.block(block_start, block_start, block_size, block_size);
 
            const bool is_stable = is_stable_pencil_block(Sb, Tb);
 
            // if the block is not stable, we need to swap it with a stable block
            // so that all stable blocks are in the top left corner of the matrix
            // @todo
 
 
            block_start += block_size;
        }
 
    }
    */
 
    template<class T, int NStates, int NInputs,
            int AOpt, int AMaxR, int AMaxC,
            int BOpt, int BMaxR, int BMaxC,
            int ROpt, int RMaxR, int RMaxC,
            int QOpt, int QMaxR, int QMaxC
    >
    Eigen::Matrix<T, NStates, NStates> dare_solver(
        const Eigen::Matrix<T, NStates, NStates, AOpt, AMaxR, AMaxC>& A,
        const Eigen::Matrix<T, NStates, NInputs, BOpt, BMaxR, BMaxC>& B,
        const Eigen::Matrix<T, NStates, NStates, QOpt, QMaxR, QMaxC>& Q,
        const Eigen::Matrix<T, NInputs, NInputs, ROpt, RMaxR, RMaxC>& R
    ){
        return dare_eigenvector_solver(A, B, Q, R);
    }
 
    template<class T, int NStates, int NInputs,
            int AOpt, int AMaxR, int AMaxC,
            int BOpt, int BMaxR, int BMaxC,
            int ROpt, int RMaxR, int RMaxC,
            int QOpt, int QMaxR, int QMaxC,
            int NOpt, int NMaxR, int NMaxC
    >
    Eigen::Matrix<T, NStates, NStates> care_solver(
        const Eigen::Matrix<T, NStates, NStates, AOpt, AMaxR, AMaxC>& A,
        const Eigen::Matrix<T, NStates, NInputs, BOpt, BMaxR, BMaxC>& B,
        const Eigen::Matrix<T, NStates, NStates, QOpt, QMaxR, QMaxC>& Q,
        const Eigen::Matrix<T, NInputs, NInputs, ROpt, RMaxR, RMaxC>& R,
        const Eigen::Matrix<T, NStates, NInputs, NOpt, NMaxR, NMaxC>& N
    ){
        // 1. Build the hamilton matrix
        const Eigen::Matrix<T, NInputs, NStates> R_inv_DT_C = R.ldlt().solve((N.transpose()));
        const Eigen::Matrix<T, NStates, NStates> A_ = A - B * R_inv_DT_C;
 
        Eigen::Matrix<T, 2*NStates, 2*NStates> H;
        H.topLeftCorner(NStates, NStates) = A_;
        H.topRightCorner(NStates, NStates) =  - B * R.ldlt().solve(B.transpose());
        H.bottomLeftCorner(NStates, NStates) = - (Q - N * R_inv_DT_C);
        H.bottomRightCorner(NStates, NStates) = -A_.transpose();
        return hamilton_solver(H);
    }
    
}