PUD: fix updating substate related information in transmitGpsInformation
[olsrd.git] / lib / pud / src / state.c
1 #include "state.h"
2
3 /* Plugin includes */
4 #include "configuration.h"
5
6 /* OLSRD includes */
7
8 /* System includes */
9 #include <pthread.h>
10
11 /*
12  * Types
13  */
14
15 /** Type describing substate */
16 typedef struct _SubStateType {
17         MovementState internalState; /**< the internal movement state */
18         unsigned long long hysteresisCounter; /**< the hysteresis counter */
19         unsigned long long hysteresisCounterToStationary; /**< the hysteresis counter threshold for changing the state to STATIONARY */
20         unsigned long long hysteresisCounterToMoving; /**< the hysteresis counter threshold for changing the state to MOVING */
21         MovementState externalState; /**< the externally visible movement state */
22 } SubStateType;
23
24 /** Type describing state */
25 typedef struct _StateType {
26         pthread_mutex_t mutex; /**< access mutex */
27         SubStateType substate[SUBSTATE_COUNT]; /**< the sub states */
28         MovementState externalState; /**< the externally visible movement state */
29 } StateType;
30
31 /*
32  * Variables
33  */
34
35 /** The state */
36 static StateType state;
37
38 /*
39  * Functions
40  */
41
42 bool initState(void) {
43         pthread_mutexattr_t attr;
44         if (pthread_mutexattr_init(&attr)) {
45                 return false;
46         }
47         if (pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE_NP)) {
48                 return false;
49         }
50         if (pthread_mutex_init(&state.mutex, &attr)) {
51                 return false;
52         }
53
54         state.substate[SUBSTATE_POSITION].internalState = MOVEMENT_STATE_STATIONARY;
55         state.substate[SUBSTATE_POSITION].hysteresisCounter = 0;
56         state.substate[SUBSTATE_POSITION].hysteresisCounterToStationary = getHysteresisCountToStationary();
57         state.substate[SUBSTATE_POSITION].hysteresisCounterToMoving = getHysteresisCountToMoving();
58         state.substate[SUBSTATE_POSITION].externalState = MOVEMENT_STATE_STATIONARY;
59         state.substate[SUBSTATE_GATEWAY].internalState = MOVEMENT_STATE_MOVING;
60         state.substate[SUBSTATE_GATEWAY].hysteresisCounter = 0;
61         state.substate[SUBSTATE_GATEWAY].hysteresisCounterToStationary = getGatewayHysteresisCountToStationary();
62         state.substate[SUBSTATE_GATEWAY].hysteresisCounterToMoving = getGatewayHysteresisCountToMoving();
63         state.substate[SUBSTATE_GATEWAY].externalState = MOVEMENT_STATE_MOVING;
64         state.externalState = MOVEMENT_STATE_MOVING; /* must comply to AND/OR conditions of sub-states */
65
66         return true;
67 }
68
69 void destroyState(void) {
70         (void) pthread_mutex_destroy(&state.mutex);
71 }
72
73 MovementState getExternalState(void) {
74         MovementState externalState;
75
76         (void) pthread_mutex_lock(&state.mutex);
77         externalState = state.externalState;
78         (void) pthread_mutex_unlock(&state.mutex);
79
80         return externalState;
81 }
82
83 /**
84  * Determine new state
85  * @param subStateIndex
86  * the sub-state for/from which the new state should be determined
87  * @param movingNow
88  * the movement result of the sub-state
89  * @param externalState
90  * a pointer to the variable in which to store the new external state
91  * @param externalStateChange
92  * a pointer to the variable in which to store whether the external state changed
93  * @param subStateExternalStateChange
94  * a pointer to the variable in which to store whether the sub-state external state changed
95  */
96 void determineStateWithHysteresis(SubStateIndex subStateIndex, TristateBoolean movingNow, MovementState * externalState,
97                 bool * externalStateChange, bool * subStateExternalStateChange) {
98         MovementState newState;
99         bool internalStateChange;
100         SubStateType * subState = &state.substate[subStateIndex];
101
102         (void) pthread_mutex_lock(&state.mutex);
103
104         /*
105          * Substate Internal State
106          */
107
108         if (movingNow == TRISTATE_BOOLEAN_SET) {
109                 newState = MOVEMENT_STATE_MOVING;
110         } else if (movingNow == TRISTATE_BOOLEAN_UNSET) {
111                 newState = MOVEMENT_STATE_STATIONARY;
112         } else {
113                 /* keep current sub-state */
114                 newState = subState->internalState;
115         }
116         internalStateChange = (subState->internalState != newState);
117         subState->internalState = newState;
118
119         /*
120          * Substate External State (+ hysteresis)
121          */
122
123         if (internalStateChange) {
124                 /* restart hysteresis for external state change when we have an internal
125                  * state change */
126                 subState->hysteresisCounter = 0;
127         }
128
129         /* when internal state and external state are not the same we need to
130          * perform hysteresis before we can propagate the internal state to the
131          * external state */
132         newState = subState->externalState;
133         if (subState->internalState != subState->externalState) {
134                 switch (subState->internalState) {
135                 case MOVEMENT_STATE_STATIONARY:
136                         /* internal state is STATIONARY, external state is MOVING */
137
138                         /* delay going to stationary a bit */
139                         subState->hysteresisCounter++;
140
141                         if (subState->hysteresisCounter >= subState->hysteresisCounterToStationary) {
142                                 /* outside the hysteresis range, go to stationary */
143                                 newState = MOVEMENT_STATE_STATIONARY;
144                         }
145                         break;
146
147                 case MOVEMENT_STATE_MOVING:
148                         /* internal state is MOVING, external state is STATIONARY */
149
150                         /* delay going to moving a bit */
151                         subState->hysteresisCounter++;
152
153                         if (subState->hysteresisCounter >= subState->hysteresisCounterToMoving) {
154                                 /* outside the hysteresis range, go to moving */
155                                 newState = MOVEMENT_STATE_MOVING;
156                         }
157                         break;
158
159                 default:
160                         /* when unknown then don't change state */
161                         break;
162                 }
163         }
164
165         *subStateExternalStateChange = (subState->externalState != newState);
166         subState->externalState = newState;
167
168         /*
169          * external state may transition into MOVING when either one of the sub-states say so (OR), and
170          * may transition into STATIONARY when all of the sub-states say so (AND)
171          */
172         *externalStateChange = false;
173         if (*subStateExternalStateChange) {
174                 bool transition;
175
176                 if (newState == MOVEMENT_STATE_STATIONARY) {
177                         /* AND: all sub-states must agree on STATIONARY */
178                         int i = 0;
179
180                         transition = true;
181                         for (i = 0; i < SUBSTATE_COUNT; i++) {
182                                 transition = transition && (state.substate[i].externalState == newState);
183                         }
184                 } else /* if (newState == MOVEMENT_STATE_MOVING) */{
185                         /* OR: one sub-state wanting MOVING is enough */
186                         transition = true;
187                 }
188
189                 if (transition) {
190                         *externalStateChange = (state.externalState != newState);
191                         state.externalState = newState;
192                 }
193         }
194
195         *externalState = state.externalState;
196
197         (void) pthread_mutex_unlock(&state.mutex);
198 }
199
200 MovementState getInternalState(SubStateIndex subStateIndex) {
201         MovementState internalState;
202
203         (void) pthread_mutex_lock(&state.mutex);
204         internalState = state.substate[subStateIndex].internalState;
205         (void) pthread_mutex_unlock(&state.mutex);
206
207         return internalState;
208 }