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 ];
            }
        }
    };
 

    struct timeDelay {
        real_t value;

        constexpr explicit timeDelay(real_t v) : value(v) {}

        constexpr size_t operator()(const real_t dt) const {
            return static_cast<size_t>( ( value/dt )  + 0.5_re);
        }

        constexpr size_t operator[](const real_t dt) const {
            return static_cast<size_t>( ( value/dt ) + 0.5_re);
        }
    };

    constexpr timeDelay operator"" _td(long double v) {
        return timeDelay(static_cast<real_t>(v));
    }

    constexpr size_t operator,(const timeDelay td, const real_t dt) {
        return static_cast<size_t>( ( td.value/dt ) + 0.5_re );
    }

    constexpr size_t delayFromTime( const real_t Time, const real_t dt )
    {
        return static_cast<size_t>( ( Time/dt ) + 0.5_re );
    }

    constexpr size_t delayFromTime( const timeDelay Time, const real_t dt )
    {
        return static_cast<size_t>( ( Time.value/dt ) + 0.5_re );
    }

    class ITransportDelay {
    public:
        virtual ~ITransportDelay() = default;
        virtual real_t delay(const real_t xInput) noexcept = 0;
        virtual real_t operator()(const real_t xInput) noexcept = 0;
        virtual size_t getNumberOfDelays() const noexcept = 0;
    };

    template<size_t numberOfDelays>
    class transportDelay : public ITransportDelay {
        private:
            real_t buf[ numberOfDelays + 1 ];
            tdl dl;
        public:
            virtual ~transportDelay() {}
            transportDelay( const real_t initValue = 0.0_re )
            {
                static_assert( numberOfDelays >= 1 , "Delay taps should be greater than 0" );
                dl.setup( buf, initValue);
            }

            real_t delay( const real_t xInput ) noexcept override
            {
                dl.insertSample( xInput );
                return dl.getOldest();
            }

            real_t operator()( const real_t xInput ) noexcept override
            {
                return delay( xInput );
            }

            size_t getNumberOfDelays() const noexcept {
                return numberOfDelays;
            }
    };

    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_MAX };
            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 ) noexcept;

            real_t operator()( const real_t u ) {
                return excite( 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 );
    };
 
 
    using customProcessModel = real_t(*)(real_t, void*);
 

    class smithPredictor {
    private:
        ltisys *model{nullptr};
        customProcessModel modelAlternate{ nullptr };
        ITransportDelay *modelDelay;
        ltisys *filter{ nullptr };
        real_t yp_hat;
        void *alternateData{ nullptr };
    public:
        virtual ~smithPredictor() {}
        smithPredictor( ltisys& modelTf,
                        ITransportDelay& mDelay,
                        const real_t initialCondition = 0.0_re )
                        : model(&modelTf), modelDelay(&mDelay), yp_hat(initialCondition) {}

        smithPredictor( customProcessModel modelCustom,
                        ITransportDelay& mDelay,
                        const real_t initialCondition = 0.0_re )
                        : modelAlternate(modelCustom), modelDelay(&mDelay), yp_hat(initialCondition) {}

        smithPredictor( ltisys& modelTf,
                        ITransportDelay& mDelay,
                        ltisys& filterTf,
                        const real_t initialCondition = 0.0_re )
                        : model(&modelTf), modelDelay(&mDelay), filter(&filterTf), yp_hat(initialCondition) {}

        smithPredictor( customProcessModel modelCustom,
                        ITransportDelay& mDelay,
                        ltisys& filterTf,
                        const real_t initialCondition = 0.0_re )
                        : modelAlternate(modelCustom), modelDelay(&mDelay), filter(&filterTf), yp_hat(initialCondition) {}

        bool updatePrediction( const real_t ut,
                               const real_t yt ) noexcept;

        real_t getPrediction() const noexcept {
            return yp_hat;
        }

        bool setFilter( ltisys& filterTf ) noexcept {
            bool retValue = filter != &filterTf;
            if ( retValue ) {
                filter = &filterTf;
            }
            return retValue;
        }

        bool setModelData( void* data ) noexcept {
            bool retValue = data != alternateData;
            if ( retValue ) {
                alternateData = data;
            }
            return retValue;
        }
    };

}
 
#endif /*QLIBS_LTISYS*/