ltisys.hpp

 
#ifndef QLIBS_LTISYS
#define QLIBS_LTISYS
 
#include <include/qlibs_types.hpp>
#include <include/tdl.hpp>
#include <include/numa.hpp>
 
 
namespace qlibs {
    /*only for continuousSystem*/
    #if !defined( LTISYS_EVAL_MODEL_CONTROLLABLE ) || !defined( LTISYS_EVAL_MODEL_OBSERVABLE )
        #define LTISYS_EVAL_MODEL_CONTROLLABLE
    #endif
    enum ltisysType {
        LTISYS_TYPE_UNKNOWN = 0,    
        LTISYS_TYPE_CONTINUOUS,     
        LTISYS_TYPE_DISCRETE,       
    };
 
    template<size_t order>
    using continuousStates = nState[ order ];
 
    template<size_t order>
    using discreteStates = real_t[ order ];
 
    template<size_t order>
    struct continuousTF {
        real_t num[ order+1 ];
        real_t den[ order+1 ];
        continuousStates<order> states = {};
        continuousTF( const real_t ( &numerator )[ order + 1 ], const real_t ( &denominator )[ order + 1 ] )
        {
            static_assert( order >= 1 , "Order should be greater than 0" );
            for ( size_t i = 0; i <= order; ++i ) {
                num[ i ] = numerator[ i ];
                den[ i ] = denominator[ i ];
            }
        }
    };
 
    constexpr size_t delayFromTime( const real_t Time, const real_t dt )
    {
        return static_cast<size_t>( ( Time/dt ) + 0.5_re );
    }
 
    template<size_t numberOfDelays>
    class transportDelay {
        private:
            real_t buf[ numberOfDelays + 1 ];
            tdl delay;
        public:
            transportDelay( const real_t initValue = 0.0_re )
            {
                static_assert( numberOfDelays >= 1 , "Delay taps should be greater than 0" );
                delay.setup( buf, initValue);
            }
            real_t operator()( const real_t xInput ) noexcept
            {
                delay.insertSample( xInput );
                return delay.getOldest();
            }
    };
 
    template<size_t delay>
    using discreteDelay = transportDelay<delay>;
 
 
    template<size_t NB, size_t NA>
    struct discreteTF {
        real_t num[ NB ];
        real_t den[ NA ];
        discreteStates<(NA>NB)? NA:NB> states = {};
        discreteTF( const real_t ( &numerator )[ NB ], const real_t ( &denominator)[ NA ] ) {
            for ( size_t i = 0; i < NB; ++i ) {
                num[ i ] = numerator[ i ];
            }
            for ( size_t i = 0; i < NA; ++i ) {
                den[ i ] = denominator[ i ];
            }
        }
    };
 
 
    class ltisys : public tdl {
        protected:
            real_t *a{ nullptr };
            real_t *b{ nullptr };
            size_t n{ 0U };
            size_t na{ 0U };
            size_t nb{ 0U };
            real_t b0{ 0.0_re };
            real_t min{ REAL_MIN };
            real_t max{ REAL_MAX };
            void normalizeTransferFunction( real_t *num,
                                            real_t *den,
                                            size_t n_num,
                                            size_t n_den );
            real_t saturate( real_t y );
            ltisysType type{ LTISYS_TYPE_UNKNOWN };
            virtual real_t update( const real_t u ) = 0;
        public:
            virtual ~ltisys() {}
            ltisys() = default;
 
            real_t excite( real_t u );
 
            virtual bool isInitialized( void ) const = 0;
 
            virtual bool setInitStates( const real_t *xi = nullptr ) = 0;
 
            ltisysType getType( void ) const
            {
                return type;
            }
 
            bool setDelay( real_t * const w,
                           const size_t nD,
                           const real_t initVal = 0.0_re ) noexcept;
 
            bool setSaturation( const real_t minV,
                                const real_t maxV ) noexcept;
    };
 
    class discreteSystem : public ltisys {
        private:
            real_t *xd{ nullptr };
            real_t update( const real_t u ) override;
        public:
            virtual ~discreteSystem() {}
 
            discreteSystem( real_t *num,
                            real_t *den,
                            real_t *x,
                            const size_t n_b,
                            const size_t n_a ) noexcept
            {
                (void)setup( num, den, x, n_b, n_a );
            }
 
            template<size_t NB, size_t NA>
            discreteSystem( real_t (&num)[ NB ],
                            real_t (&den)[ NA ],
                            real_t *x ) noexcept
            {
                (void)setup( num, den, x, NB, NA );
            }
 
            template<size_t NB, size_t NA>
            discreteSystem( discreteTF<NB, NA>& dtf ) noexcept
            {
                (void)setup( dtf.num, dtf.den, dtf.states, NB, NA );
            }
 
            bool setup( real_t *num,
                        real_t *den,
                        real_t *x,
                        const size_t n_b,
                        const size_t n_a ) noexcept;
 
            template<size_t NB, size_t NA>
            bool setup( real_t (&num)[ NB ],
                        real_t (&den)[ NA ],
                        real_t *x )
            {
                return setup( num, den, x, NB, NA );
            }
 
            template<size_t NB, size_t NA>
            bool setup( discreteTF<NB, NA>& dtf ) noexcept
            {
                return setup( dtf.num, dtf.den, dtf.states, NB, NA );
            }
 
            bool isInitialized( void ) const override
            {
                return ( nullptr != xd ) && ( LTISYS_TYPE_DISCRETE == type );
            }
 
            bool setInitStates( const real_t *xi = nullptr ) override;
 
            static real_t updateFIR( real_t *w,
                                     const size_t wsize,
                                     const real_t x,
                                     const real_t * const c = nullptr );
    };
 
    class continuousSystem : public ltisys {
        private:
            real_t dt{ 1.0_re };
            nState *xc{ nullptr };
            real_t update( const real_t u ) override;
        public:
            virtual ~continuousSystem() {}
 
            continuousSystem( real_t *num,
                              real_t *den,
                              nState *x,
                              const size_t nD,
                              const real_t dT ) noexcept
            {
                (void)setup( num, den, x, nD, dT );
            }
 
            template<size_t order>
            continuousSystem( continuousTF<order>& ctf, const real_t dT )
            {
                (void)setup( ctf.num, ctf.den, ctf.states, order, dT );
            }
 
            template<size_t systemOrder>
            continuousSystem( real_t (&num)[ systemOrder+1 ],
                              real_t (&den)[ systemOrder+1 ],
                              nState (&x)[ systemOrder ],
                              const real_t dT ) noexcept
            {
                (void)setup( num, den, x, systemOrder, dT );
            }
 
            bool setup( real_t *num,
                        real_t *den,
                        nState *x,
                        const size_t nD,
                        const real_t dT ) noexcept;
 
            template<size_t systemOrder>
            bool setup( real_t (&num)[ systemOrder+1 ],
                        real_t (&den)[ systemOrder+1 ],
                        nState (&x)[ systemOrder ],
                        const real_t dT ) noexcept
            {
                return setup( num, den, x, systemOrder, dT );
            }
 
            template<size_t order>
            bool setup( continuousTF<order>& ctf, const real_t dT )
            {
                return setup( ctf.num, ctf.den, ctf.states, order, dT );
            }
 
            bool isInitialized( void ) const override
            {
                return ( nullptr != xc ) && ( LTISYS_TYPE_CONTINUOUS == type );
            }
 
            bool setInitStates( const real_t *xi = nullptr ) override;
 
            bool setIntegrationMethod( integrationMethod m );
    };
 
}
 
#endif /*QLIBS_LTISYS*/