artis-star/src/artis-star/kernel/pdevs/qss/Integrator.hpp

/**
 * @file kernel/pdevs/qss/Integrator.hpp
 * @author The ARTIS Development Team
 * See the AUTHORS or Authors.txt file
 */

/*
 * ARTIS - the multimodeling and simulation environment
 * This file is a part of the ARTIS environment
 *
 * Copyright (C) 2013-2019 ULCO http://www.univ-littoral.fr
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#ifndef QSS_INTEGRATOR
#define QSS_INTEGRATOR

#include <artis-star/kernel/pdevs/Dynamics.hpp>
#include <artis-star/kernel/pdevs/qss/Data.hpp>

namespace artis {
    namespace pdevs {
        namespace qss {

            struct IntegratorParameters {
                double x_0;
            };

            template<class Time>
            class Integrator
                    : public artis::pdevs::Dynamics<Time, Integrator<Time>, IntegratorParameters> {

                typedef enum {
                    INIT,
                    WAIT_FOR_QUANTA,
                    WAIT_FOR_X_DOT,
                    WAIT_FOR_BOTH,
                    RUNNING
                } State;

            public:
                enum inputs {
                    QUANTA, X_DOT, RESET
                };

                enum outputs {
                    OUT
                };

                typedef enum vars {
                    VALUE
                } Observable;

                enum states {
                    STATE,
                    LAST_OUT_DATE,
                    UP_THRESHOLD,
                    DOWN_THRESHOLD,
                    LAST_OUT_VALUE,
                    INIT_VALUE,
                    CURRENT_VALUE,
                    EXPECTED_VALUE,
                    ARCHIVE_X_DOT,
                    ARCHIVE_DATE
                };

                Integrator(const std::string& name,
                        const Context<Time, Integrator<Time>, IntegratorParameters>& context)
                        :
                        artis::pdevs::Dynamics<Time, Integrator<Time>, IntegratorParameters>(name,
                                context)
                {
                    DECLARE_STATES(int, ((STATE, &Integrator<Time>::_state)));
                    DECLARE_STATES(typename Time::type,
                            ((LAST_OUT_DATE, &Integrator<Time>::_last_output_date)));
                    DECLARE_STATES(double, ((UP_THRESHOLD, &Integrator<Time>::_up_threshold),
                            (DOWN_THRESHOLD, &Integrator<Time>::_down_threshold),
                            (LAST_OUT_VALUE, &Integrator<Time>::_last_output_value),
                            (INIT_VALUE, &Integrator<Time>::_init_value),
                            (CURRENT_VALUE, &Integrator<Time>::_current_value),
                            (EXPECTED_VALUE, &Integrator<Time>::_expected_value)));
                    DECLARE_STATES(std::vector<double>,
                            ((ARCHIVE_X_DOT, &Integrator<Time>::_archive_x_dot)));
                    DECLARE_STATES(std::vector<typename Time::type>,
                            ((ARCHIVE_DATE, &Integrator<Time>::_archive_date)));

                    this->input_ports({
                            {QUANTA, "quanta"},
                            {X_DOT,  "x_dot"},
                            {RESET,  "reset"}});
                    this->output_port({OUT, "out"});
                    this->observable({VALUE, "value"});

                    _init_value = context.parameters().x_0;
                }

                virtual ~Integrator() { }

                virtual void dconf(typename Time::type t, typename Time::type e,
                        const common::Bag<Time>& bag)
                {
                    dint(t);
                    dext(t, e, bag);
                }

                virtual void dint(const typename Time::type& time)
                {
                    switch (_state) {
                    case RUNNING: {
                        double last_derivative_value = _archive_x_dot.back();

                        _last_output_value = _expected_value;
                        _last_output_date = time;
                        _archive_x_dot.clear();
                        _archive_date.clear();
                        _archive_x_dot.push_back(last_derivative_value);
                        _archive_date.push_back(time);
                        _current_value = _expected_value;
                        _state = WAIT_FOR_QUANTA;
                        break;
                    }
                    case INIT: {
                        _state = WAIT_FOR_BOTH;
                        _last_output_value = _current_value;
                        _last_output_date = time;
                        break;
                    }
                    default:
                        assert(false);
                    }
                }

                virtual void dext(const typename Time::type& t, const typename Time::type& e,
                        const common::Bag<Time>& bag)
                {
                    bool reset = false;

                    std::for_each(bag.begin(), bag.end(),
                            [this, t, e, &reset](const common::ExternalEvent<Time>& event) {
                                if (event.on_port(QUANTA)) {
                                    QuantifierData data;

                                    event.data()(data);
                                    _up_threshold = data.up;
                                    _down_threshold = data.down;
                                    if (_state == WAIT_FOR_QUANTA) {
                                        _state = RUNNING;
                                    }
                                    if (_state == WAIT_FOR_BOTH) {
                                        _state = WAIT_FOR_X_DOT;
                                    }
                                } else if (event.on_port(X_DOT)) {
                                    DerivativeData data;

                                    event.data()(data);
                                    _archive_x_dot.push_back(data.x_dot);
                                    _archive_date.push_back(t);
                                    if (_state == WAIT_FOR_X_DOT) {
                                        _state = RUNNING;
                                    }
                                    if (_state == WAIT_FOR_BOTH) {
                                        _state = WAIT_FOR_QUANTA;
                                    }
                                } else if (event.on_port(RESET)) {
                                    IntegratorData data;

                                    event.data()(data);
                                    _current_value = data.value;
                                    reset = true;
                                    _archive_x_dot.clear();
                                    _archive_date.clear();
                                }
                            });
                    if (reset) {
                        _state = INIT;
                    } else {
                        if (_state == RUNNING) {
                            _current_value = current_value(t);
                            _expected_value = expected_value(t);
                        }
                    }
                }

                virtual void start(const typename Time::type& /* time */)
                {
                    _current_value = _init_value;
                    _state = INIT;
                }

                virtual typename Time::type ta(const typename Time::type& t)
                {
                    double current_derivative;

                    switch (_state) {
                    case INIT:
                        return 0;
                    case RUNNING:

                        assert(_archive_date.size() > 0);

                        current_derivative = _archive_x_dot.back();
                        if (current_derivative == 0) {
                            return Time::infinity;
                        }
                        if (current_derivative > 0) {

                            assert(_up_threshold - _current_value >= 0);

                            return (_up_threshold - _current_value) / current_derivative;
                        } else {

                            assert(_down_threshold - _current_value <= 0);

                            return (_down_threshold - _current_value) / current_derivative;
                        }
                    default:
                        return Time::infinity;
                    }
                }

                virtual common::Bag<Time> lambda(const typename Time::type& /* time */) const
                {
                    common::Bag<Time> msgs;

                    switch (_state) {
                    case RUNNING: {
                        const IntegratorData data = {_expected_value};

                        msgs.push_back(common::ExternalEvent<Time>(OUT, data));
                        break;
                    }
                    case INIT: {
                        const IntegratorData data = {_current_value};

                        msgs.push_back(common::ExternalEvent<Time>(OUT, data));
                        break;
                    }
                    default:
                        break;
                    }
                    return msgs;
                }

                virtual common::Value observe(const typename Time::type& /* t */,
                        unsigned int index) const
                {
                    switch (index) {
                    case VALUE:
                        return (double) (_current_value);
                    default:
                        return common::Value();
                    }
                }

            private:
                double current_value(const typename Time::type& time) const
                {
                    double val = _last_output_value;

                    if (_archive_date.size() > 0) {
                        for (size_t i = 0; i < _archive_date.size() - 1; i++) {
                            val +=
                                    (_archive_date[i + 1] - _archive_date[i]) * _archive_x_dot[i];
                        }
                        val += (time - _archive_date.back()) * _archive_x_dot.back();
                    }
                    return val;
                }

                double expected_value(const typename Time::type& /* time */) const
                {
                    double current_derivative = _archive_x_dot.back();

                    if (current_derivative == 0) {
                        return _current_value;
                    } else if (current_derivative > 0) {
                        return _up_threshold;
                    }
                    return _down_threshold;
                }

                int _state;

                typename Time::type _last_output_date;

                double _up_threshold;
                double _down_threshold;

                double _last_output_value;
                double _init_value;
                double _current_value;
                double _expected_value;

                std::vector<double> _archive_x_dot;
                std::vector<typename Time::type> _archive_date;
            };

        }
    }
}

#endif