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

  info(F("Booting up..."));
}
void updateStartupState() {
  stateMachine.transitionTo(idleState);
}
void leaveStartupState() {

// -------------- Idle State ---------------
void enterIdleState() {
  info(F("Idling, ready for commands..."));
  allStop();
}
void updateIdleState() {}
void leaveIdleState() {}

// -------------- Forward State ---------------

// Move the bot forward at full speed
void enterForwardState() {
  allStart();
  allSpeedAhead();
}
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"));
}

