 // -------------- Idle State ---------------
 void enterIdleState() {
   info(F("Idling, ready for commands..."));
-  digitalWrite(13, HIGH);
+  allStop();
 }
-void updateIdleState() {
+void updateIdleState() {}
+void leaveIdleState() {}
 
+// -------------- Forward State ---------------
+
+// Move the bot forward at full speed
+void enterForwardState() {
+  allStart();
+  allSpeedAhead();
 }
-void leaveIdleState() {
+void updateForwardState() {}
+void leaveForwardState() {
+  allStop();
+}
 
+// -------------- Backward State ---------------
+
+// Move the bot backward at full speed
+void enterBackwardState() {
+  allStart();
+  allSpeedReverse();
+}
+void updateBackwardState() {}
+void leaveBackwardState() {
+  allStop();
+}
+
+
+// ******************* HELPERS *******************
+
+// aREST API listener
+int setControlState(String command) {
+  info(F("Handling a REST request"));
+  info(command);
+  if (command.startsWith("stop")) {
+     stateMachine.transitionTo(idleState);
+     return 2;
+  }
+  if (command.startsWith("forward")) {
+     stateMachine.transitionTo(forwardState);
+     return 3;
+  }
+  if (command.startsWith("backward")) {
+     stateMachine.transitionTo(backwardState);
+     return 4;
+  }
+  return 1; 
 }
 
 // ******************* HELPERS *******************
+
+// -------------- Motor Driver Helpers ---------------
+// disable the motors via the motor driver STANDBY
+void allStop() {
+  warn(F("enabling STANDBY"));
+}
+// disable the motors via the motor driver STANDBY
+void allStart() {
+  warn(F("disabling STANDBY"));
+}
+void allSpeedAhead() {
+  warn(F("moving forward"));
+}
+void allSpeedReverse() {
+  warn(F("moving backward"));
+}
+

// FSM states
State startupState = State(enterStartupState,updateStartupState,leaveStartupState);
State idleState = State(enterIdleState,updateIdleState,leaveIdleState);

FSM stateMachine = FSM(startupState);

void setup() {
  startLog();
}

void loop() {
  stateMachine.update();
}

// ******************* FSM state callback methods *******************

#include <FiniteStateMachine.h>
#include <SPI.h>
#include <aREST.h>
#include "LogHelpers.h"
#include "TimingHelpers.h"

