Merge branch 'libusb'

[eurobot/public.git] / src / fsm / fsm.h

/**
 * @file fsm.h
 * @author Tran Duy Khanh
 * @author Michal Sojka
 * @date 07/04/17
 *
 * @brief Finite State Machine
 */

/* Copyright: (c) 2007 CTU Dragons
 *            CTU FEE - Department of Control Engineering
 * Contacts: Tran Duy Khanh <trandk1@fel.cvut.cz>
 * License: GNU GPL v.2
 */

/**
\defgroup fsm Finite State Machines

Finite state machines/automatons (FSM) are used to control various
parts of the %robot. 

Content:
- \ref fsm_general
- \ref state_functions
- \ref fsm_events
- \ref fsm_usage
- \ref subfsm

\section fsm_general Introduction

FSMs are used to write applications that need to react on events. In
the simples cases it is sufficient to write such applications only by
combinations of switch and if statements. In other cases when the
number of events is higher than a small number and the response to
events differs depending on many conditions, such approach is usually
error prone, since the code becomes complicated. This library provides
a framework which makes programming such applications more easy.

The fundamental entity in this library is FSM. Every FSM is defined by
structure ::robo_fsm and a set of state functions.  The state
functions are executed as a response to some events. The events can be
generated internally (see ::fsm_common_events), by another FSM or by
some other piece of code.  There is exactly one "current" state
function at a given time. The goal of the current state functions is
to react to events. The reaction to an event can be represented by an
arbitrary piece of code that does something useful. It is also
possible to change the current state (function) of FSM (so called
state transition). Having several state functions, it is possible to
react differently on the same events depending on what is the current
state.

Every FSM can be executed either in its dedicated threads or together
with other FSMs in a single thread. Which way is better for your
application depends on your needs. If the code in state functions may
take long time to execute and you need to react quickly to some
events, use the threaded FSMs and assign higher priority to threads
with real-time requirements and low priority to non-real-time threads
with long execution times.

On the other hand, having all FSMs running in a single thread may
simplify your application, because you do not have to deal with
concurrent execution. The price for this is, that response time to an
critical event may be too high.

It is also possible to have something in between the above two
approaches. You can have, for instance, real-time FSMs running in
their own high-priority threads and non-real-time FSMs, sharing a
single low-priority thread.

\section state_functions State Functions

The state function is a function taking a pointer to FSM structure as
an argument and returning an integer value. To make declaration of
state function easier, there is a macro FSM_STATE(). This macro
declares the state functions and also adds some code to conditionally
print debugging messages. Thanks to this macro, programmer can
concentrate on the real programming task and the complexity of
declaration is hidden.

The usual structure of a state function is one big switch
statement. The switch condition is the #FSM_EVENT macro, whose value
is the identifier of the event (see section \ref fsm_events) to be
handled. In the handling code you can execute whatever you want. If
there is some data associated with the event, it can be retrieved by
#FSM_EVENT_PTR or #FSM_EVENT_INT. Additionally, FSM infrastructure
provides some useful functions and macros. To work with time, you can
use FSM_TIMER(). To execute transition to another state, use
FSM_TRANSITION() or SUBFSM_TRANSITION() (see \ref subfsm). To send an
event to another FSM, use FSM_SIGNAL().

An example state function, whose only function is to wait 1 second
before entering \c another_state is shown here:

\code
FSM_STATE(my_state) {
  switch (FSM_EVENT) {
    case EV_ENTRY:
      FSM_TIMER(1000);
      break;
    case EV_TIMER:
      FSM_TRANSITION(another_state);
      break;
  }
}
\endcode

For the above to work, it is important to declare the macro @c FSM_<id>
before including header with event definition and fsm.h. See the note in \ref
fsm_events section. This means, that state functions for different
FSMs have to be declared in separate files.

\section fsm_events Events

An event is identified by an event identifier. This is a string
usually starting with @c EV_ prefix. Some data can be associated with
events. This data can be a pointer or integer typecasted to pointer.

Every FSM can handle so called common events (e.g. #EV_ENTRY,
see ::fsm_common_events) and also some additional events specific to
the FSM. Those additional events are declared only for a given FSM and
can be sent to that FSM from another place by the FSM_SIGNAL()
macro. To avid programming errors we have three requirements for
working with events:

-# It is not possible to send an event to FSM, which didn't declare
   it. This situation should produce compile-time error.
-# In a state function, an attempt to handle an event declared for
   another FSM should also produce compile-time error.
-# If some event, which can be handled by the FSM, is omitted in the switch
   command, we want to be warned.

These requirements are hard to achieve in plain C. Therefore,
additional events for FSMs are declared as a data structure in Python
and there is a Python script eventgen.py, which converts this definition to .h a
.c files.

The event definition is a Python dictionary (associative array) called
@c events. Keys to this dictionary are the lowercase identifiers of
FSM. The value is another dictionary, which defines the events for
that FSM. The key is the event identifier and value is a comment. In
the example below, event for two FSMs called "tested" and "tester" are
defined. The "tested" FSM can handle additional events EV_PING and EV_OTHER and
"tester" FSM handles "EV_PONG".

\include example_ev.py

The script, which generates .c and .h source code is called
eventgen.py. In .h, there is one enum for each FSM with all events
this FSM can handle. The members of enums are not directly event identifiers,
but some internal identifiers starting with double underscore
(__). You should never use these identifiers directly in your
application. Then, there are macros, which translates the event
identifiers to internal event identifiers. Thanks to this, the above
requirements 1. and 2. are fulfilled. Requirement no. 3 is also
fulfilled because recent versions of GCC warns if some enum member is
not handled in switch statement (unless you use @c default statement).

@note You have to always include the generated header file with events before fsm.h.
If you do not do so, an error is generated.

@note In the source, where you define your state function, you have to
define macro @c FSM_&lt;id&gt;, where @c &lt;id&gt; is uppercase identifier of FSM
as defined in event definition. If you do not do so, you get a compilation error
on definition of state function. GCC generates this error message:
@verbatim
error: expected ‘=’, ‘,’, ‘;’, ‘asm’ or ‘__attribute__’ before ‘FSM_XXX_not_defined’
@endverbatim

\section fsm_usage Usage

To use FSMs provided by this library in your application, you need to
-# define additional event for your automatons (see section \ref fsm_events),
-# define state functions for your automatons (see section \ref
state_functions) and
-# start execution of the automatons (every FSM executes in its own thread).

Definition of state functions usually starts with lines like this:
@code
#define FSM_SOME_NAME // define which FSM state functions are to be declared here
#include "events.h"   // include definition of additional events (generated from events.py)
#include <fsm.h>

FSM_STATE(test_state1) {
        ...
}
@endcode

Initialization of an automaton looks like:
@code
struct robo_fsm fsm;
struct fsm_main_loop loop;
fsm_main_loop_init(&loop);
fsm_init(&fsm, "id_for_debug_massages", &loop);
fsm.state = &fsm_state_test_state1;       // Initial state of the automaton
@endcode

To start the FSM in its own thread do:
@code
if (fsm_start(&loop, NULL) != 0) {
        perror("fsm_start");
        exit(1);
}
@endcode
To start it in the current thread do:
@code
fsm_main_loop(&loop);
@endcode

\section subfsm Sub-Automatons

Sometimes, it may be useful to enter a state of sub-automaton and
return back to current state after this sub-automaton finishes its
task. You can enter the sub-automaton state by calling
SUBFSM_TRANSITION(). When the sub-automaton is done, call from some of
its state functions SUBFSM_RET(). Then the original state function
will be called with ::EV_RETURN.

Sub-automaton calling can be nested up to the level of 10.

 */



#ifndef __robofsm__
#define __robofsm__

#include <stdlib.h>
#include <stdbool.h>
#include <pthread.h>
#include <semaphore.h>
#include <sys/time.h>
#include <errno.h>
#include "fsm_common_events.h"
#include <stdio.h>

/**
 * Type for FSM events
 * @ingroup fsm
 */
typedef struct fsm_event {
        int id;                 /** Event identifier (e.g. EV_TIMER) */
        void *data;             /** Any data sent together with the event */
} fsm_event;

#ifndef __FSM_ALIASES_DEFINED
#define EV_NOEVENT        __COMMON_EV_NOEVENT
#define EV_ENTRY          __COMMON_EV_ENTRY
#define EV_EXIT           __COMMON_EV_EXIT
#define EV_RETURN         __COMMON_EV_RETURN
#define EV_TIMER          __COMMON_EV_TIMER
#endif

struct robo_fsm;

struct fsm_main_loop {
        pthread_t threadid;     /**< Thread running the main loop or NULL. */
        struct robo_fsm *fsm; /**< List of FSMs handled by this loop */

        /** Main loop waits on this semaphore while it waits for an event. */
        sem_t event_sent;
        /** Pointer to FSM whose timer expires first or NULL on no
         * timer is active. */
        struct robo_fsm *first;
        struct timespec now;    /**< Time when an event occurred */
};

/** Event handler function for FSM.
 * @ingroup fsm
 */
typedef int (*robo_fsm_state_t)(struct robo_fsm *fsm);

#define FSM_EVENT_QUEUE_LEN 10

/** Set on event queue overrun, never zeroed. */
#define FSM_FLAG_EVENT_LOST     0x01
/** Set by fsm_exit() */
#define FSM_FLAG_FINISH         0x02

/** Structure describing FSM.
 * @ingroup fsm
 */
struct robo_fsm {
        struct robo_fsm *next;
        /** Main loop, which dispatches events for this FSM. */
        struct fsm_main_loop *loop;
        unsigned flags;
        pthread_mutex_t send_mutex;
         /** Ring-buffer for incomming events */
        struct fsm_event events[FSM_EVENT_QUEUE_LEN];
        /** Index in the @a events of the next event to be processed.  */
        unsigned ev_head;       
        /** Index in the @a events where to put events for this
         * message. Protected by @a send_mutex */
        unsigned ev_tail;   

        struct timespec timer;  /**< Time of next timer expiration */

        /** Pointer to current state function. This function is called
         * when an event occurs. */
        robo_fsm_state_t state;
        const char *state_name;                  /**< Name of the current state  */
        robo_fsm_state_t last_state; /**< Pointer to previous state function */
        /** Stack of sub-automaton states for use by FCALL() and SUBFSM_TRANSITION(). */
        const char *last_state_name;             /**< Name of the last state  */
        robo_fsm_state_t fnc_stack[10];
        /** Pointer to the top of fnc_stack */
        robo_fsm_state_t *fnc_sp;
        int debug_states;       /**< Whether to print debug messages on state transitions. */
        char *debug_name;       /**< The name of automaton (printed in debug messages) */
        /**
         * Pointer to transition callback function. This function is
         * called whenever fsm changes state. This callback can be
         * used to track/debug the state of FSM. @c fsm->state_name
         * will contain the name of current state, @c fsm->last_state_name
         * the name of last state.
         */
        void (*transition_callback)(struct robo_fsm *fsm);
};      /* robo_fsm */

#ifdef __cplusplus
extern "C" {
#endif

void fsm_main_loop_init(struct fsm_main_loop *loop);
void fsm_init(struct robo_fsm *fsm, char *debug_name, struct fsm_main_loop *loop);
int fsm_start(struct fsm_main_loop *loop, pthread_attr_t *attr);
void fsm_exit(struct robo_fsm *fsm);
void fsm_destroy(struct robo_fsm *fsm);
void fsm_main_loop(struct fsm_main_loop *loop);
void __fsm_timespec_add_ms(struct timespec *ts, struct timespec *now, long ms);
void __fsm_timespec_invalidate(struct timespec *ts);
int __fsm_timespec_cmp(struct timespec *ts1, struct timespec *ts2);



/* FSM (Finite State Machine) */
/* do nothing */
int fsm_nop_state(struct robo_fsm *fsm);

/**
 * Returns string with the name of event submitted as parameter.
 * @param ev Indetifier of the event
 * @return The name of event or "undefined event!!!" if the @c ev
 * doesn't refer to a valid event.
 *
 * @ingroup fsm
 */
const char *fsm_event_str(fsm_event ev);
const char *fsm_common_event_str(fsm_event ev);

/** \name Return codes of state functions */
//@{
/** After return, FSM thread starts waiting for the new event. */
#define RC_WAIT         0
/** After return, current state function (fnc_act) will be immediately
 * called again. Used by FSM_TRANSITION(). */
#define RC_PROC         1
//@}

#define FSM_WAIT_FOREVER -1l

static inline void fsm_set_flags(struct robo_fsm *fsm, int flag_mask)
{
        __sync_fetch_and_or(&fsm->flags, flag_mask);
}

 static inline void __fsm_signal(struct robo_fsm *fsm, fsm_event event,
                                 bool sem)
{
        unsigned new_tail;
        pthread_mutex_lock(&fsm->send_mutex);
        fsm->events[fsm->ev_tail] = event;
        new_tail = fsm->ev_tail + 1;
        if (new_tail >= FSM_EVENT_QUEUE_LEN) 
                new_tail = 0;
        
        if (new_tail != fsm->ev_head) {
                fsm->ev_tail = new_tail;
                if (sem && fsm->loop)
                        sem_post(&fsm->loop->event_sent);
        } else {
                fsm_set_flags(fsm, FSM_FLAG_EVENT_LOST);
                fprintf(stderr, "fsm %s: Event lost\n", fsm->debug_name);
        }
        pthread_mutex_unlock(&fsm->send_mutex);
}

/**
 * Sends an event to another FSM, which may run  in another
 * thread. Event sending is asynchronous, but each FSM has a buffer for
 * several events. If this buffer is full, @a event_lost flag is set
 * and the event is lost. This should never happen in a carefully
 * designed application.
 *
 * @note This macro ensures that it is not possible to send an event
 * to other FSM than the one for which it was declared. If you get
 * compilation error, that @c __<FSM_ID>_<EVENT_ID> is not declared,
 * it means the recipient doesn't handle this event.
 *
 * @ingroup fsm
 * @param fsm_id Event recipient's ID.
 * @param event Event ID to send.
 * @param data Any data (pointer or typecasted int) sent together with
 *             the event.
 */
#define FSM_SIGNAL(fsm_id, event, data)                                   \
        do {                                                              \
                fsm_event e = {__##fsm_id##_##event, data};               \
                __fsm_signal(FSM_GET_BY_ID(fsm_id), e, true);             \
        } while(0)

/**
 * \def FSM_GET_BY_ID(fsm_id)
 * 
 * Returns pointer to struct ::fsm identified by the id. This macro
 * should be defined by the application, which uses fsm.h. 
 * 
 * @param fsm_id FSM indentifier. This is the uppercase variant of the
 * string, which defines the FSM in definition of events in python
 * source.
 *
 * @ingroup fsm
 */

#define DBG_STATE()     DBG_FSM_STATE(__FUNCTION__)
#define DBG_FSM_STATE(name)     do { if (fsm->debug_states) printf("fsm %s: %s(%s)\n", fsm->debug_name, name, fsm_event_str(fsm->events[fsm->ev_head])); } while(0)
/**
 * Prints current event in a human readable form.
 * @ingroup fsm
 */
#define DBG_PRINT_EVENT(msg)    printf("fsm %s %s %s: %s\n", fsm->debug_name, fsm->state_name, fsm_event_str(fsm->events[fsm->ev_head]), msg)

#ifdef __cplusplus
}
#endif

static inline void call_transition_callback(struct robo_fsm *fsm)
{
        if (fsm->transition_callback != NULL &&
            (fsm->last_state != (fsm)->state) &&
            (fsm->events[fsm->ev_head].id == EV_ENTRY || /* FIXME: Is this line correct? */
             fsm->events[fsm->ev_head].id == EV_RETURN)) {
                fsm->transition_callback(fsm);
        }
}

/**********************/
/* High level FSM API */
/**********************/

/**
 * Specifies state function visibility.
 *
 * If this macro is redefined from "" (empty) to static, then the
 * state functions are declared as static.
 */
#define FSM_STATE_VISIBILITY

#define ___fsm_func_name(fsm, state, suf) fsm_state_##fsm##_##state##suf
#define __fsm_func_name(fsm, state, suf) ___fsm_func_name(fsm, state, suf)

/**
 * Defines a state function for finite state machine. This function is
 * called by FSM framework whenever some event occurs. The event can
 * be read by using ::FSM_EVENT macro. In the function body, other @a
 * FSM_xxx macros can be used to invoke state transition or set
 * the timer. When no state transition is invoked, FSM waits
 * for the next event, after the function returns. Then, after another
 * event occurs, this state function is called again with that event.
 *
 * @note The body of state function is declared as void inline
 * function. This function is called from state function stub, which
 * contains debugging macros.
 *
 * The actual name of the function stub is @c fsm_state_<name> and the
 * name of inline implementation is @c fsm_state_<name>_impl.
 *
 * @param name Name of the state.
 * @ingroup fsm
 */
#ifdef __FSM_NAME
#define FSM_STATE(name)                                                 \
        static inline void __fsm_func_name(__FSM_NAME, name, _impl)(struct robo_fsm *fsm, int *__ret); \
        FSM_STATE_VISIBILITY int __fsm_func_name(__FSM_NAME, name,)(struct robo_fsm *fsm) { \
                int ret=RC_WAIT;                                        \
                DBG_FSM_STATE(#name);                                   \
                fsm->state_name = #name;                                \
                call_transition_callback(fsm);                          \
                __fsm_func_name(__FSM_NAME, name, _impl)(fsm, &ret);    \
                return ret;                                             \
        }                                                               \
        static inline void __fsm_func_name(__FSM_NAME, name, _impl)(struct robo_fsm *fsm, int *__ret)
#else
#define FSM_STATE(name) error_FSM_XXX_not_defined
#endif
/**
 * Declares a prototype of state function defined later.
 * @ingroup fsm
 * @param name Name of state function.
 */
#ifdef __FSM_NAME
#define FSM_STATE_DECL(name) \
FSM_STATE_VISIBILITY int __fsm_func_name(__FSM_NAME, name,)(struct robo_fsm *fsm)
#else
#define FSM_STATE_DECL(name) error_FSM_XXX_not_defined
#endif

/**
 * Declares a prototype of state function of given FSM. This can be
 * used to declare state function in header files (of other .c files)
 * that the one with the state functions itself
 * 
 * @ingroup fsm
 * @param fsm_name Name of FSM
 * @param name Name of state function.
 */
#define FSM_STATE_FULL_DECL(fsm_name, name)                     \
int __fsm_func_name(fsm_name, name,)(struct robo_fsm *fsm)

/**
 * Sets FSM's one-shot timer. After the specified number of
 * milliseconds elapses, ::EV_TIMER is generated, unless
 * FSM_TIMER_STOP() is used. The timer is automatically stopped on
 * every trnasition.
 *
 * @param ms Time to timer expiration in milliseconds.
 * @ingroup fsm
 */
#define FSM_TIMER(ms) \
        if (FSM_EVENT != EV_EXIT) {                                     \
                __fsm_timespec_add_ms(&fsm->timer, &fsm->loop->now, (ms)); \
                if (!fsm->loop->first ||                                \
                    __fsm_timespec_cmp(&fsm->timer,                     \
                                       &fsm->loop->first->timer) == -1) { \
                        fsm->loop->first = fsm;                         \
                }                                                       \
        }

/**
 * Stops the timer.
 * @ingroup fsm
 */
#define FSM_TIMER_STOP() __fsm_timer_stop(fsm)

void __fsm_timer_stop(struct robo_fsm *);

#ifndef __FSM_EVENT_ENUM
/* This macro is normally declared in automatically generated event
 * header file. If fsm.h is included from other files, we fall back to
 * fsm_common_events. If you get warning: "__FSM_EVENT_ENUM"
 * redefined, it means that FSM_xxx definition of event is included
 * after fsm.h, which is not correct. */
#define __FSM_EVENT_ENUM enum fsm_common_events
#endif
/**
 * Value of current event in state functions.
 * @ingroup fsm
 */
#define FSM_EVENT ((__FSM_EVENT_ENUM)(fsm->events[fsm->ev_head].id))

/**
 * Value of the pointer sent together with the event.
 * @ingroup fsm
 */
#define FSM_EVENT_PTR (fsm->events[fsm->ev_head].data)

/**
 * Value of the integer data sent together with the event.
 * @ingroup fsm
 */
#define FSM_EVENT_INT ((long int)(fsm->events[fsm->ev_head].data))

/**
 * Sets current state to @c next. The state is not changed
 * immediately. The rest of current state function finishes its
 * execution and then, if @c next is not the current state, the
 * current state function is called again with ::EV_EXIT
 * event. Finally a call to the @c next state function (even if it is
 * the same as current) is executed with ::EV_ENTRY as event.
 *
 * @param next Next state.
 * @ingroup fsm
 */
#ifdef __FSM_NAME
#define FSM_TRANSITION(next)                                            \
        do {                                                            \
                if (fsm->events[fsm->ev_head].id != EV_EXIT) {          \
                        fsm_event e = {EV_ENTRY, NULL};                 \
                        fsm->last_state = fsm->state;                   \
                        fsm->last_state_name = fsm->state_name;         \
                        fsm->state = &__fsm_func_name(__FSM_NAME, next,); \
                        fsm->events[fsm->ev_head] = e;                  \
                        *__ret=RC_PROC;                                 \
                }                                                       \
        } while(0)
#else
#define FSM_TRANSITION(next) error_FSM_XXX_not_defined
#endif

/**
 * Invoke state transition to sub-FSM. The behaviour is almost the
 * same as of FSM_TRANSITION(), but whenever the sub-FSM returns (see
 * ::SUBFSM_RET), the current state function is executed with
 * ::EV_RETURN as event. Moreover, the current state function is not
 * called with ::EV_EXIT before the transtition.
 *
 * @param substate Initial state of sub-FSM.
 * @param data Data passed to the sub-FSM.
 * @ingroup fsm
 */
#ifdef __FSM_NAME
#define SUBFSM_TRANSITION(substate, data)                               \
        do {                                                            \
                if (fsm->events[fsm->ev_head].id != EV_EXIT) {          \
                        fsm_event e = {EV_ENTRY, data};                 \
                        *(fsm->fnc_sp++) = fsm->state;                  \
                        fsm->last_state = fsm->state;                   \
                        fsm->last_state_name = fsm->state_name;         \
                        fsm->state = &__fsm_func_name(__FSM_NAME, substate,); \
                        fsm->events[fsm->ev_head] = e;                  \
                        *__ret = RC_PROC;                               \
                }                                                       \
        } while(0)
#else
#define SUBFSM_TRANSITION(substate, data) error_FSM_XXX_not_defined
#endif
/**
 * Return from sub-FSM. ::EV_EXIT is executed for the current state
 * and ::EV_RETURN for the state where it is returning.
 *
 * @param data Data returned back to the caller
 * @ingroup fsm
 */
#define SUBFSM_RET(data)                                                \
        do {                                                            \
                if (fsm->events[fsm->ev_head].id != EV_EXIT) {          \
                        fsm_event e = {EV_RETURN, data};                \
                        fsm->last_state = fsm->state;                   \
                        fsm->last_state_name = fsm->state_name;         \
                        fsm->state = *(--fsm->fnc_sp);                  \
                        fsm->events[fsm->ev_head] = e;                  \
                        *__ret = RC_PROC;                               \
                }                                                       \
        } while(0)

#endif  /* __robofsm__ */