FiniteStateMachine.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX 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/>.
 *
 * @package    ArmarXCore
 * @author     Christian Dreher <c.dreher@kit.edu>
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#pragma once
 
 
// STD/STL
#include <exception>
#include <functional>
#include <map>
#include <memory>
#include <optional>
#include <tuple>
 
namespace armarx
{
 
 
    template <typename T, typename ChainType, typename StatusType, StatusType failure_status>
    class FiniteStateMachine
    {
 
    public:
        using state = std::function<StatusType(T*)>;
        static const StatusType failure_status_value = failure_status;
 
    protected:
        mutable T* m_context;
        std::optional<state> m_start_state;
        std::optional<state> m_init_from_state;
        std::optional<state> m_init_to_state;
        std::optional<state> m_default_fail_state;
        std::map<long, std::optional<state>> m_states;
        std::map<std::tuple<long, StatusType>, long> m_transitions;
 
    public:
        FiniteStateMachine(T& context)
        {
            m_context = &context;
        }
 
        FiniteStateMachine(std::shared_ptr<T> context)
        {
            m_context = *context;
        }
 
        FiniteStateMachine(T* context)
        {
            m_context = context;
        }
 
        ChainType&
        start_from(state start_state)
        {
            m_start_state = start_state;
            return *static_cast<ChainType*>(this);
        }
 
        ChainType&
        from(state from_state)
        {
            if (not m_init_from_state)
            {
                m_init_from_state = from_state;
            }
            else
            {
                throw std::logic_error{"From-state was already set.  Did you forget to append "
                                       "on_success() or on_failure()?"};
            }
 
            return *static_cast<ChainType*>(this);
        }
 
        ChainType&
        to(state to_state)
        {
            if (not m_init_to_state)
            {
                m_init_to_state = to_state;
            }
            else
            {
                throw std::logic_error{"To-state was already set.  Did you forget to append "
                                       "on_success() or on_failure()?"};
            }
 
            return *static_cast<ChainType*>(this);
        }
 
        ChainType&
        on(StatusType status)
        {
            if (not m_init_from_state or not m_init_to_state)
            {
                throw std::logic_error{"Both from-state and to-state must be set with "
                                       "from(...).to(...)."};
            }
 
            // Get the ID's of the two functions.
            const long id1 = get_id(m_init_from_state);
            const long id2 = get_id(m_init_to_state);
 
            // Make sure that the states are known.
            if (m_states.find(id1) == m_states.end())
            {
                m_states[id1] = m_init_from_state.value();
            }
            if (m_states.find(id2) == m_states.end())
            {
                m_states[id2] = m_init_to_state.value();
            }
 
            // Add transitions.
            m_transitions[std::make_tuple(id1, status)] = id2;
 
            m_init_from_state = std::nullopt;
            m_init_to_state = std::nullopt;
 
            return *static_cast<ChainType*>(this);
        }
 
        ChainType&
        on_failure()
        {
            return on(failure_status_value);
        }
 
        ChainType&
        on_any_failure()
        {
            if (m_init_from_state)
            {
                throw std::logic_error{"When using on_any_failure(), from-state must be unset."};
            }
 
            m_default_fail_state = m_init_to_state;
            m_init_to_state = std::nullopt;
 
            return *static_cast<ChainType*>(this);
        }
 
        virtual StatusType
        run() const
        {
            if (m_init_from_state.has_value() or m_init_to_state.has_value())
            {
                throw std::logic_error{"From-state or to-state set but not commited with .on()"};
            }
 
            if (not m_start_state.has_value())
            {
                throw std::logic_error{"Cannot run without a start state."};
            }
 
            long current_state_id;
            StatusType status;
            std::optional<state> next_state = m_start_state;
 
            do
            {
                current_state_id = get_id(next_state);
                status = next_state.value()(m_context);
                next_state = get_next_state(current_state_id, status);
            } while (status != failure_status_value and next_state.has_value());
 
            if (status == failure_status_value)
            {
                if (not next_state.has_value() and m_default_fail_state.has_value())
                {
                    next_state = m_default_fail_state;
                }
 
                status = next_state.value()(m_context);
            }
 
            return status;
        }
 
    protected:
        std::optional<state>
        get_next_state(const long state_id, const StatusType status) const
        {
            auto key = std::make_tuple(state_id, status);
            if (m_transitions.find(key) != m_transitions.end())
            {
                const long target_id = m_transitions.at(key);
                return m_states.at(target_id);
            }
 
            return std::nullopt;
        }
 
        long
        get_id(std::optional<state> fn) const
        {
            return get_id(fn.value());
        }
 
        long
        get_id(const state& fn) const
        {
            return *(long*)(char*)&fn;
        }
    };
 
    template <typename T>
    class BoolFiniteStateMachine :
        public FiniteStateMachine<T, BoolFiniteStateMachine<T>, bool, false>
    {
 
    public:
        using FiniteStateMachine<T, BoolFiniteStateMachine<T>, bool, false>::FiniteStateMachine;
 
        BoolFiniteStateMachine&
        on_success()
        {
            this->on(true);
            return *this;
        }
    };
 
 
    enum class status
    {
        success,
        failure
    };
 
    template <typename T>
    class BinaryFiniteStateMachine :
        public FiniteStateMachine<T, BinaryFiniteStateMachine<T>, status, status::failure>
    {
 
    public:
        using FiniteStateMachine<T, BinaryFiniteStateMachine<T>, status, status::failure>::
            FiniteStateMachine;
 
        BinaryFiniteStateMachine&
        on_success()
        {
            this->on(status::success);
            return *this;
        }
    };
 
    /**
     * User-defined template argument deductions.
     */
    template <typename T>
    BoolFiniteStateMachine(T*) -> BoolFiniteStateMachine<T>;
    template <typename T>
    BoolFiniteStateMachine(T&) -> BoolFiniteStateMachine<T>;
    template <typename T>
    BoolFiniteStateMachine(std::shared_ptr<T>) -> BoolFiniteStateMachine<T>;
    template <typename T>
    BinaryFiniteStateMachine(T*) -> BinaryFiniteStateMachine<T>;
    template <typename T>
    BinaryFiniteStateMachine(T&) -> BinaryFiniteStateMachine<T>;
    template <typename T>
    BinaryFiniteStateMachine(std::shared_ptr<T>) -> BinaryFiniteStateMachine<T>;
 
 
} // namespace armarx