--- a/Discovery/Src/motion.c	Tue Jun 11 05:31:07 2019 +0200
+++ b/Discovery/Src/motion.c	Mon Jun 17 19:46:18 2019 +0200
@@ -10,250 +10,123 @@
 #include <math.h>
 #include "motion.h"
 
-#define PITCH_HISTORY_ENTRIES		20	/* number of pitch value stored in buffer */
 #define	STABLE_STATE_COUNT			2	/* number of count to declare a state as stable (at the moment based on 100ms) */
-#define MOVE_DELTA_COUNT			10	/* Delta count needed to identify a valid movement */
-#define SHAKE_DELTA_COUNT			30	/* Delta count needed to identify a valid minima / maxima */
-
-
-typedef enum
-{
-		RELATIVE_MOVE_STATIC = 0,
-		RELATIVE_MOVE_INCREASE,
-		RELATIVE_MOVE_DECREASE,
-		RELATIVE_MOVE_INVALID
-} relativeMove_t;
-
-
-
-
+#define STABLE_STATE_TIMEOUT		5	/* Detection shall be aborted if a movement state is stable for more than 500ms */
+#define MOVE_DELTA_SPEED			4	/* Delta speed needed to identify a valid movement */
+#define SHAKE_DELTA_COUNT			10	/* Delta count needed to identify a valid minima / maxima */
+#define SHAKE_DELTA_END				10	/* Delta allowed between start and end position */
 
-float pitchHistory[PITCH_HISTORY_ENTRIES];			/* Ringbuffer of last read pich values */
-static uint8_t pitchWriteIdx;			/* Index of current write slot */
-
-/* Init data structures */
-void InitMotion()
-{
-	uint8_t tmp;
-
-	for(tmp = 0; tmp < PITCH_HISTORY_ENTRIES; tmp++)
-	{
-		pitchHistory[tmp] = 0;
-	}
-	pitchWriteIdx = 0;
-
-}
-
-detectionState_t detectionState = DETECT_NOTHING;
+static detectionState_t detectionState = DETECT_NOTHING;
 
 /* Detect if user is generating an pitch including return to starting position (shake) */
 /* This is done by feeding the past movements value per value into a state machine */
 detectionState_t detectShake(float currentPitch)
 {
-	static uint8_t runningIdx = 0;
 	static uint8_t stableCnt = 0;
 	static float lastPitch = 0.0;
 	static float startPitch = 0.0;
-
-	relativeMove_t relativeMove = RELATIVE_MOVE_INVALID;
-
-
-	pitchHistory[pitchWriteIdx] = currentPitch;
-	runningIdx = pitchWriteIdx;
-#if 0
-	runningIdx = pitchWriteIdx + 1;
-
-	if(runningIdx == PITCH_HISTORY_ENTRIES)
-	{
-		runningIdx = 0;
-	}
-#endif
+	static float minmax = 0.0;
+	float curSpeed;
 
 	if((detectionState == DETECT_NEG_SHAKE) || (detectionState == DETECT_POS_SHAKE))	/* discard last detection */
 	{
 		detectionState = DETECT_NOTHING;
 	}
-//	do
-//	{
-//		lastPitch = pitchHistory[runningIdx];
-#if 0
-		runningIdx++;
-		if(runningIdx == PITCH_HISTORY_ENTRIES)
-		{
-			runningIdx = 0;
-		}
-#endif
-	/* define relative movement compared to last position */
-		if(fabsf(lastPitch -  pitchHistory[runningIdx]) < MOVE_DELTA_COUNT )		/* more or less no movement */
-		{
-			relativeMove = RELATIVE_MOVE_STATIC;
-			stableCnt++;
-		}
-		else
-		{
-			if(lastPitch > pitchHistory[runningIdx])				/* decreasing */
-			{
-				relativeMove = RELATIVE_MOVE_DECREASE;
-			}
-			else
-			{
-				relativeMove = RELATIVE_MOVE_INCREASE;				/* increasing */
-			}
-		}
+
+	curSpeed = currentPitch - lastPitch;
 
-	/* feed value into statemachine */
-		switch (detectionState)
-		{
-			case DETECT_NOTHING: 	if(relativeMove == RELATIVE_MOVE_STATIC)
+	/* feed value into state machine */
+	switch (detectionState)
+	{
+			case DETECT_NOTHING: 	if(fabsf(curSpeed) < MOVE_DELTA_SPEED)	/* detect a stable condition before evaluating for the next move */
 									{
-										if(stableCnt > 4)
-										{
-											detectionState = DETECT_START;
-											startPitch = lastPitch;
-										}
+										stableCnt++;
+									}
+									if(stableCnt > STABLE_STATE_COUNT)
+									{
+										detectionState = DETECT_START;
+										stableCnt = 0;
 									}
 				break;
-			case DETECT_START:		switch(relativeMove)
+			case DETECT_START:		if(fabsf(curSpeed) > MOVE_DELTA_SPEED)
 									{
-										case RELATIVE_MOVE_INCREASE: detectionState = DETECT_POS_MOVE;
-											break;
-										case RELATIVE_MOVE_DECREASE: detectionState = DETECT_NEG_MOVE;
-											break;
-										default:
-											break;
+										if(curSpeed > 0)
+										{
+											detectionState = DETECT_POS_MOVE;
+										}
+										else
+										{
+											detectionState = DETECT_NEG_MOVE;
+										}
+										stableCnt = 0;
+										startPitch = lastPitch;
 									}
 				break;
-			case DETECT_POS_MOVE:	switch(relativeMove)
-									{
-										case RELATIVE_MOVE_INCREASE: detectionState = DETECT_POS_MOVE;
-											break;
-										case RELATIVE_MOVE_STATIC: if(fabsf(startPitch - lastPitch) > SHAKE_DELTA_COUNT)
-																	{
-																		detectionState = DETECT_MAXIMA;
-																	}
-																	else
-																	{
-																		detectionState = DETECT_NOTHING;
-																	}
-											break;
-										default:
-											detectionState = DETECT_NOTHING;
-											break;
-									}
-				break;
-			case DETECT_NEG_MOVE:	switch(relativeMove)
+			case DETECT_NEG_MOVE:
+			case DETECT_POS_MOVE:	if(fabsf(curSpeed) > MOVE_DELTA_SPEED )
 									{
-										case RELATIVE_MOVE_DECREASE: detectionState = DETECT_NEG_MOVE;
-											break;
-										case RELATIVE_MOVE_STATIC:	if(fabsf(startPitch - lastPitch) > SHAKE_DELTA_COUNT)   /* significant movment */
-																	{
-																		detectionState = DETECT_MINIMA;
-																	}
-																	else
-																	{
-																		detectionState = DETECT_NOTHING;
-																	}
-											break;
-										default:
-											detectionState = DETECT_NOTHING;
-											break;
-									}
-				break;
-			case DETECT_MAXIMA:		if((relativeMove != RELATIVE_MOVE_STATIC) && (stableCnt < STABLE_STATE_COUNT ))
-									{
-										detectionState = DETECT_NOTHING;
+										stableCnt++;
 									}
 									else
 									{
-										if(relativeMove == RELATIVE_MOVE_DECREASE)
+										if(stableCnt >= STABLE_STATE_COUNT)	/* debounce movement */
 										{
-											detectionState = DETECT_FALLBACK;
+											if(fabsf(startPitch - currentPitch) > SHAKE_DELTA_COUNT)
+											{
+												detectionState++;
+												minmax = lastPitch;
+											}
+											else
+											{
+												detectionState = DETECT_NOTHING;
+											}
 										}
-										if(relativeMove == RELATIVE_MOVE_INCREASE)
+										else
 										{
-											detectionState = DETECT_POS_MOVE;
+											detectionState = DETECT_NOTHING;
 										}
+										stableCnt = 0;
 									}
 				break;
-			case DETECT_MINIMA:		if((relativeMove != RELATIVE_MOVE_STATIC) && (stableCnt < STABLE_STATE_COUNT ))
+			case DETECT_MINIMA:
+			case DETECT_MAXIMA:		if(fabsf(currentPitch - minmax ) < SHAKE_DELTA_COUNT)		/* stay at maximum for short time to add a pattern for user interaction */
 									{
-										detectionState = DETECT_NOTHING;
+										stableCnt++;
 									}
 									else
 									{
-										if(relativeMove == RELATIVE_MOVE_DECREASE)
+										if(stableCnt > 0)
 										{
-											detectionState = DETECT_NEG_MOVE;
-										}
-										if(relativeMove == RELATIVE_MOVE_INCREASE)
-										{
-											detectionState = DETECT_RISEBACK;
+											detectionState++;
 										}
-									}
-				break;
-
-			case DETECT_FALLBACK:	switch(relativeMove)
-									{
-										case RELATIVE_MOVE_DECREASE: detectionState = DETECT_FALLBACK;
-											break;
-										case RELATIVE_MOVE_STATIC:  if( stableCnt >= STABLE_STATE_COUNT)
-																	{
-																		if(fabsf(startPitch - lastPitch) < MOVE_DELTA_COUNT) 		/* are we where started, again? */
-																		{
-																			detectionState = DETECT_POS_SHAKE;
-																			memset(pitchHistory, 0, sizeof(pitchHistory));
-																		}
-																		else
-																		{
-																			detectionState = DETECT_START;							/* start new detection */
-																			startPitch = lastPitch;
-																		}
-																	}
-											break;
-										default:
+										else
+										{
 											detectionState = DETECT_NOTHING;
-											break;
+										}
+										stableCnt = 0;
 									}
 				break;
-			case DETECT_RISEBACK:	switch(relativeMove)
+			case DETECT_RISEBACK:
+			case DETECT_FALLBACK:	if(fabsf(curSpeed) < MOVE_DELTA_SPEED)
 									{
-										case RELATIVE_MOVE_INCREASE: detectionState = DETECT_RISEBACK;
-											break;
-										case RELATIVE_MOVE_STATIC:  if(stableCnt >= STABLE_STATE_COUNT)
-																	{
-																		if(fabsf(startPitch - lastPitch) < MOVE_DELTA_COUNT)
-																		{
-																			detectionState = DETECT_NEG_SHAKE;
-																			memset(pitchHistory, 0, sizeof(pitchHistory));
-																		}
-																		else
-																		{
-																			detectionState = DETECT_START;
-																			startPitch = lastPitch;
-																		}
-																	}
-											break;
-										default:
-											detectionState = DETECT_NOTHING;
-											break;
+										if(fabsf(startPitch - currentPitch) < SHAKE_DELTA_END)
+										{
+											detectionState++;
+										}
 									}
+									stableCnt++;
+				break;
+			default:
+				detectionState = DETECT_NOTHING;
 				break;
 
-			default:
-				break;
-
-		}
-		if(relativeMove != RELATIVE_MOVE_STATIC)		/* reset counter for stable time detection */
-		{
-			stableCnt = 0;
-		}
-//	} while ((runningIdx != pitchWriteIdx) && (detectionState != DETECT_NEG_SHAKE) && (detectionState != DETECT_POS_SHAKE));
+	}
+	if(stableCnt > STABLE_STATE_TIMEOUT)
+	{
+		detectionState = DETECT_NOTHING;
+		stableCnt = 0;
+	}
 
 	lastPitch = currentPitch;
-	pitchWriteIdx++;
-	if(pitchWriteIdx == PITCH_HISTORY_ENTRIES)
-	{
-		pitchWriteIdx = 0;
-	}
 	return detectionState;
 }